|
1 #!/usr/bin/python |
|
2 # 14.2 |
|
3 import os, sys |
|
4 sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"] |
|
5 |
|
6 import scipy as sp |
|
7 from scipy import linalg |
|
8 from pend import cont_mat, delta_a, acker |
|
9 from polyfuncs import polmul |
|
10 |
|
11 A = sp.array([[1, 2], [0, 3]]) |
|
12 c = sp.array([1, 0]) |
|
13 p = sp.roots([1, -0.5, 0.5]) |
|
14 b = sp.array([[0], [1]]) |
|
15 K = acker(A, b, p) |
|
16 |
|
17 p1 = 0.1 + 0.1j |
|
18 p2 = 0.1 - 0.1j |
|
19 phi = sp.real(sp.convolve([1, -p1], [1, -p2])) |
|
20 Obs = sp.vstack((c, sp.dot(c, A))) |
|
21 alphae = sp.dot(A, A)-0.2*A+0.02*sp.eye(2,2) |
|
22 Lp = sp.dot(alphae, sp.dot(linalg.inv(Obs), sp.array([[0],[1]]))) |
|
23 |
|
24 C = sp.array([[1, 0, 0.5, 2], [0, 1, -4.71, 2.8]]) |
|
25 dC = 1 |
|
26 |
|
27 [HD,dHD] = polmul(K,0,C,dC) |
|
28 [HD,dHD] = polmul(HD,dHD,Lp,0) |
|
29 |
|
30 print HD, dHD |