--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ss/pend.py	Fri May 27 14:24:59 2011 +0530
@@ -0,0 +1,76 @@
+#!/usr/bin/python
+# 14.1
+# Uses A, B from 2.1 (pend_model.py)
+
+import os, sys
+sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"]
+
+import scipy as sp
+from scipy import signal
+from scipy import linalg
+from dscr import dscr
+from pend_model import pend_model
+
+def pol2cart(theta, r):
+    x = r*sp.cos(theta)
+    y = r*sp.sin(theta)
+    return x, y
+
+def cont_mat(a, b):
+    n = len(a)
+    con = b
+    for i in range(1, n):
+        prod = sp.identity(n)
+        for j in range(i):
+            prod = sp.dot(prod, a)
+        prod = sp.dot(prod, b)
+        con = sp.hstack((con, prod))
+    return con
+
+def delta_a(a, P):
+    n = len(a)
+    delta_a = sp.identity(n)
+    for pole in P:
+        delta_a = sp.dot(delta_a, (a - pole*sp.identity(n)))
+    return delta_a
+
+def acker(a, b, P):
+    """ Crude implementation of ackermann's formula """
+    con = cont_mat(a, b)
+    con_inv = linalg.inv(con)
+    del_a = delta_a(a, P)
+    e_n = sp.zeros(len(a))
+    e_n[-1] = 1
+
+    K = sp.dot(sp.dot(e_n, con_inv), del_a)
+    return K
+
+if __name__ == "__main__":
+    A, B = pend_model()
+    # Changed form of C, D from the scilab one to make it work with signal.lti
+    C = sp.eye(1, 4)  
+    D = 0
+    Ts = 0.01
+    G = signal.lti(A, B, C, D)
+    H = dscr(G, Ts)
+    a, b, c, d = H.A, H.B, H.C, H.D
+    rise = 5
+    epsilon = 0.1
+    N = rise/Ts
+    omega = sp.pi/2/N
+    r = epsilon**(omega/sp.pi)
+    r1 = r
+    r2 = 0.9*r
+
+    x1, y1 = pol2cart(omega, r1)
+    x2, y2 = pol2cart(omega, r2)
+
+    p1 = x1+y1*1j
+    p2 = x2+y2*1j
+    p3 = x1-y1*1j
+    p4 = x2-y2*1j
+    P = [[p1], [p2], [p3], [p4]]
+    
+    K = acker(a,b,P) 
+
+    print K