From 99302cdb44558d757865cd3a15c450a9d0d2ed8c Mon Sep 17 00:00:00 2001 From: arnold Date: Tue, 2 Jan 2018 17:17:18 -0700 Subject: [PATCH 1/6] Modify statefbk.place to use the YT algorithm implemented in scipy.signals.place_poles --- control/statefbk.py | 62 +++++++++++++++++++--------------- control/tests/statefbk_test.py | 19 +++++++++++ 2 files changed, 53 insertions(+), 28 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index 634922131..e6802ae0f 100644 --- 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,7 +64,23 @@ 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 -------- @@ -71,35 +88,24 @@ def place(A, B, p): >>> 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 def acker(A, B, poles): diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 34070aca7..ef4a171d8 100644 --- 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 From 186d6d8e8468cbd8b5d7c0c435dbb9f085e5455f Mon Sep 17 00:00:00 2001 From: arnold Date: Tue, 2 Jan 2018 18:27:49 -0700 Subject: [PATCH 2/6] YT algo cannot place multiple poles at the same location when rk(B)=1. Also, aadd tests for control dimension checks. --- control/tests/matlab_test.py | 3 +-- control/tests/statefbk_test.py | 8 ++++++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/control/tests/matlab_test.py b/control/tests/matlab_test.py index 8c8a1f199..e625a9f3e 100644 --- a/control/tests/matlab_test.py +++ b/control/tests/matlab_test.py @@ -400,9 +400,8 @@ def testModred(self): modred(self.siso_ss3, [1], 'matchdc') modred(self.siso_ss3, [1], 'truncate') - @unittest.skipIf(not slycot_check(), "slycot not installed") 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]) @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 ef4a171d8..8cdcafece 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""" @@ -168,7 +168,7 @@ def testPlace(self): [1.136, 1.136], [0, 0,], [-3.146, 0]]) - P = np.array([-0.2, -0.5, -5.0566, -8.6659]) + 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 @@ -176,6 +176,10 @@ def testPlace(self): 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 From 311e9e2d5330bc1d45dd38f2c00183db961290ad Mon Sep 17 00:00:00 2001 From: arnold Date: Wed, 3 Jan 2018 12:45:50 -0700 Subject: [PATCH 3/6] Keep the original place code in a new function, place_varga. The scipy implementation of YT stays in place() proper --- control/statefbk.py | 72 +++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 69 insertions(+), 3 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index e6802ae0f..6443a5ea5 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -45,13 +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 @@ -64,7 +64,7 @@ def place(A, B, p): Returns ------- K : 2-d array - Gains such that A - B K has eigenvalues given in p + Gain such that A - B K has eigenvalues given in p Algorithm --------- @@ -107,6 +107,72 @@ def place(A, B, p): 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]) + """ + + # Make sure that SLICOT is installed + try: + from slycot import sb01bd + except ImportError: + 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); + 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 + # Contributed by Roberto Bucher def acker(A, B, poles): """Pole placement using Ackermann method From 100eb1f85542ae24382ac6dfe165af2cb4a05ff7 Mon Sep 17 00:00:00 2001 From: arnold Date: Wed, 3 Jan 2018 16:02:49 -0700 Subject: [PATCH 4/6] Added 'see also' to doc-strings --- control/statefbk.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/control/statefbk.py b/control/statefbk.py index 6443a5ea5..e2f02c1a4 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -87,6 +87,10 @@ def place(A, B, p): >>> 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 @@ -142,6 +146,10 @@ def place_varga(A, B, p): >>> A = [[-1, -1], [0, 1]] >>> B = [[0], [1]] >>> K = place(A, B, [-2, -5]) + + See Also: + -------- + place, acker """ # Make sure that SLICOT is installed From 45fdea97f426b5f37aa13b139ca524277a70ad04 Mon Sep 17 00:00:00 2001 From: arnold Date: Thu, 4 Jan 2018 09:15:02 -0700 Subject: [PATCH 5/6] Added tests for acker, and place_varga. Added an assert_raises test for repeated poles in place. Added placeholder tests in matlab_test for place and acker and reverted the repeated pole test for place varga. --- control/tests/matlab_test.py | 8 ++++++++ control/tests/statefbk_test.py | 21 +++++++++++++++++++++ 2 files changed, 29 insertions(+) diff --git a/control/tests/matlab_test.py b/control/tests/matlab_test.py index e625a9f3e..1793dee16 100644 --- a/control/tests/matlab_test.py +++ b/control/tests/matlab_test.py @@ -400,9 +400,17 @@ def testModred(self): modred(self.siso_ss3, [1], 'matchdc') 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.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): (K, S, E) = lqr(self.siso_ss1.A, self.siso_ss1.B, np.eye(2), np.eye(1)) diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 8cdcafece..b998e4548 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -180,6 +180,27 @@ def testPlace(self): 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) + + 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 From 70ecf7544b15a0b913eb04db0d92c234990e860b Mon Sep 17 00:00:00 2001 From: arnold Date: Thu, 4 Jan 2018 09:31:28 -0700 Subject: [PATCH 6/6] Forgot to check that slycot is installed in testPlace_varga() --- control/tests/statefbk_test.py | 1 + 1 file changed, 1 insertion(+) diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index b998e4548..042bda701 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -185,6 +185,7 @@ def testPlace(self): 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.]])