added acker() from Roberto Bucher

murrayrm · murrayrm · commit c4adc4c94759 · 2012-10-04T04:46:55.000Z
diff --git a/ChangeLog b/ChangeLog
@@ -1,3 +1,9 @@
+2012-09-09  Richard Murray  <murray@altura.local>
+
+	* tests/statefbk_test.py (TestStatefbk.testAcker): unit test
+
+	* src/statefbk.py (acker): added acker() from Roberto Bucher.
+
 2012-08-29  Richard Murray  <murray@altura.local>
 
 	* src/xferfcn.py (TransferFunction._common_den): fixed up code for
diff --git a/src/__init__.py b/src/__init__.py
@@ -67,7 +67,7 @@
 from nichols import nichols_plot, nichols
 from phaseplot import phase_plot, box_grid
 from rlocus import root_locus
-from statefbk import place, lqr, ctrb, obsv, gram
+from statefbk import place, lqr, ctrb, obsv, gram, acker
 from statesp import StateSpace
 from timeresp import forced_response, initial_response, step_response, \
     impulse_response
diff --git a/src/statefbk.py b/src/statefbk.py
@@ -41,6 +41,7 @@
 
 # External packages and modules
 import numpy as np
+import scipy as sp
 import ctrlutil
 from exception import *
 import statesp
@@ -99,6 +100,44 @@ def place(A, B, p):
     # Return the gain matrix, with MATLAB gain convention
     return -F
 
+# Contributed by Roberto Bucher <roberto.bucher@supsi.ch>
+def acker(A,B,poles):
+    """Pole placemenmt using Ackermann method
+
+    Call:
+    k=acker(A,B,poles)
+
+    Parameters
+    ----------
+    A, B : State and input matrix of the system
+    poles: desired poles
+
+    Returns
+    -------
+    k: matrix
+    State feedback gains
+
+    """
+    # Convert the inputs to matrices
+    a = np.mat(A)
+    b = np.mat(B)
+
+    # Make sure the system is controllable
+    p = np.real(np.poly(poles))
+    ct = ctrb(A,B)
+    if sp.linalg.det(ct) == 0:
+        raise ValueError, "System not reachable; pole placement invalid"
+
+    # Place the poles using Ackermann's method
+    n = np.size(p)
+    pmat = p[n-1]*a**0
+    for i in np.arange(1,n):
+        pmat = pmat+p[n-i-1]*a**i
+    k = sp.linalg.inv(ct)*pmat
+    k = k[-1][:]
+
+    return k
+
 def lqr(*args, **keywords):
     """Linear quadratic regulator design
 
diff --git a/tests/statefbk_test.py b/tests/statefbk_test.py
@@ -5,10 +5,20 @@
 
 import unittest
 import numpy as np
-from control.statefbk import ctrb, obsv, place, lqr, gram
+from control.statefbk import ctrb, obsv, place, lqr, gram, acker
 from control.matlab import *
 
 class TestStatefbk(unittest.TestCase):
+    """Test state feedback functions"""
+
+    def setUp(self):
+        # Maximum number of states to test + 1
+        self.maxStates = 5
+        # Maximum number of inputs and outputs to test + 1
+        self.maxTries = 4
+        # Set to True to print systems to the output.
+        self.debug = False
+
     def testCtrbSISO(self):
         A = np.matrix("1. 2.; 3. 4.")
         B = np.matrix("5.; 7.")
@@ -83,6 +93,34 @@ def testGramsys(self):
         self.assertRaises(ValueError, gram, sys, 'o')
         self.assertRaises(ValueError, gram, sys, 'c')
 
+    def testAcker(self):
+        for states in range(1, self.maxStates):
+            for i in range(self.maxTries):
+                # start with a random SS system and transform to TF then
+                # back to SS, check that the matrices are the same.
+                sys = rss(states, 1, 1)
+                if (self.debug):
+                    print sys
+
+                # Make sure the system is not degenerate
+                Cmat = ctrb(sys.A, sys.B)
+                if (np.linalg.matrix_rank(Cmat) != states):
+                    if (self.debug):
+                        print "  skipping (not reachable)"
+                        continue
+
+                # Place the poles at random locations
+                des = rss(states, 1, 1);
+                poles = pole(des)
+
+                # Now place the poles using acker 
+                K = acker(sys.A, sys.B, poles)
+                new = ss(sys.A - sys.B * K, sys.B, sys.C, sys.D)
+                placed = pole(new)
+
+                np.testing.assert_array_almost_equal(np.sort(poles), 
+                                                     np.sort(placed))
+
 def suite():
    return unittest.TestLoader().loadTestsFromTestCase(TestStatefbk)
 
