|
1 #!/usr/bin/python |
|
2 |
|
3 import sys |
|
4 sys.path += ['/media/all/work/digital_control/pycode/python/'] |
|
5 |
|
6 import scipy as sp |
|
7 from polyfuncs import polmul, putin, ext |
|
8 from xdync import xdync |
|
9 from specfac import specfac |
|
10 |
|
11 def lqg_as(A,degA,B,degB,C,degC,k,rho,V,degV,W,degW,F,degF): |
|
12 """ LQG controller design by method of Ahlen and Sternad. """ |
|
13 r, b, degb = specfac(A,degA,B,degB,rho,V,degV,W,degW,F,degF) |
|
14 WFA = sp.convolve(A, sp.convolve(F,W))[::-1] |
|
15 dWFA = degW + degF + degA |
|
16 rhs1, drhs1 = polmul(W, degW, WFA, dWFA) |
|
17 print rhs1, "rhs1", drhs1 |
|
18 print C, "C", degC |
|
19 rhs1, drhs1 = polmul(rhs1, drhs1, C, degC) |
|
20 rhs1 = rho * rhs1 |
|
21 rhs2 = sp.convolve(C, sp.convolve(V, sp.convolve(B,V)[::-1])) |
|
22 drhs2 = degC + 2*degV + degB |
|
23 for i in range(degb - degB - degV): |
|
24 rhs2 = sp.convolve(rhs2, [0,1]) |
|
25 drhs2 = drhs2 + degb - degB - degV |
|
26 C1 = sp.zeros((1,2)) |
|
27 |
|
28 C1, degC1 = putin(C1,0,rhs1,drhs1,0,0) |
|
29 C1, degC1 = putin(C1,degC1,rhs2,drhs2,0,1) |
|
30 rbf = r * b[::-1] |
|
31 D1 = sp.zeros((2, 2)) |
|
32 D1, degD1 = putin(D1,0,rbf,degb,0,0) |
|
33 for i in range(k): |
|
34 rbf = sp.convolve(rbf, [0, 1]) |
|
35 D1, degD1 = putin(D1,degD1,rbf,degb+k,1,1) |
|
36 N = sp.zeros((1, 2)) |
|
37 N, degN = putin(N,0,-B,degB,0,0) |
|
38 |
|
39 AF, dAF = polmul(A,degA,F,degF) |
|
40 N, degN = putin(N,degN,AF,dAF,0,1) |
|
41 |
|
42 Y, degY, X, degX = xdync(N, degN, D1, degD1, C1, degC1)[:4] |
|
43 |
|
44 R, degR = ext(X,degX,0,0) |
|
45 S, degS = ext(X,degX,0,1) |
|
46 X = Y[::-1] |
|
47 |
|
48 return R, degR, S, degS |
|
49 |
|
50 if __name__ == "__main__": |
|
51 A, dA = sp.array([1, -0.44]), 1 |
|
52 B, dB = sp.array([0.51, 1.21]), 1 |
|
53 F, dF = sp.array([1, -1]), 1 |
|
54 C, dC = sp.array([1, -0.44]),1 |
|
55 k = 1 |
|
56 V, W = 1, 1 |
|
57 dV, dW = 0, 0 |
|
58 rho = 1 |
|
59 int1 = 1 |
|
60 |
|
61 print lqg_as(A,dA,B,dB,C,dC,k,rho,V,dV,W,dW,F,dF) |