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