|
1 #!/usr/bin/python |
|
2 # 14.1 |
|
3 # Uses A, B from 2.1 (pend_model.py) |
|
4 |
|
5 import os, sys |
|
6 sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"] |
|
7 |
|
8 import scipy as sp |
|
9 from scipy import signal |
|
10 from scipy import linalg |
|
11 from dscr import dscr |
|
12 from pend_model import pend_model |
|
13 |
|
14 def pol2cart(theta, r): |
|
15 x = r*sp.cos(theta) |
|
16 y = r*sp.sin(theta) |
|
17 return x, y |
|
18 |
|
19 def cont_mat(a, b): |
|
20 n = len(a) |
|
21 con = b |
|
22 for i in range(1, n): |
|
23 prod = sp.identity(n) |
|
24 for j in range(i): |
|
25 prod = sp.dot(prod, a) |
|
26 prod = sp.dot(prod, b) |
|
27 con = sp.hstack((con, prod)) |
|
28 return con |
|
29 |
|
30 def delta_a(a, P): |
|
31 n = len(a) |
|
32 delta_a = sp.identity(n) |
|
33 for pole in P: |
|
34 delta_a = sp.dot(delta_a, (a - pole*sp.identity(n))) |
|
35 return delta_a |
|
36 |
|
37 def acker(a, b, P): |
|
38 """ Crude implementation of ackermann's formula """ |
|
39 con = cont_mat(a, b) |
|
40 con_inv = linalg.inv(con) |
|
41 del_a = delta_a(a, P) |
|
42 e_n = sp.zeros(len(a)) |
|
43 e_n[-1] = 1 |
|
44 |
|
45 K = sp.dot(sp.dot(e_n, con_inv), del_a) |
|
46 return K |
|
47 |
|
48 if __name__ == "__main__": |
|
49 A, B = pend_model() |
|
50 # Changed form of C, D from the scilab one to make it work with signal.lti |
|
51 C = sp.eye(1, 4) |
|
52 D = 0 |
|
53 Ts = 0.01 |
|
54 G = signal.lti(A, B, C, D) |
|
55 H = dscr(G, Ts) |
|
56 a, b, c, d = H.A, H.B, H.C, H.D |
|
57 rise = 5 |
|
58 epsilon = 0.1 |
|
59 N = rise/Ts |
|
60 omega = sp.pi/2/N |
|
61 r = epsilon**(omega/sp.pi) |
|
62 r1 = r |
|
63 r2 = 0.9*r |
|
64 |
|
65 x1, y1 = pol2cart(omega, r1) |
|
66 x2, y2 = pol2cart(omega, r2) |
|
67 |
|
68 p1 = x1+y1*1j |
|
69 p2 = x2+y2*1j |
|
70 p3 = x1-y1*1j |
|
71 p4 = x2-y2*1j |
|
72 P = [[p1], [p2], [p3], [p4]] |
|
73 |
|
74 K = acker(a,b,P) |
|
75 |
|
76 print K |