--- /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