|
1 #!/usr/bin/python |
|
2 # 13.3 |
|
3 import os, sys |
|
4 sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"] |
|
5 |
|
6 import scipy as sp |
|
7 from polyfuncs import polmul, poladd |
|
8 |
|
9 def specfac(A, dA, B, dB, rho, V, dV, W, dW, F, dF): |
|
10 """Implements the spectral factorization for use with LQG |
|
11 control by design method of Ahlen and Sternad""" |
|
12 |
|
13 AFW = sp.convolve(A, sp.convolve(W, F)) |
|
14 dAFW = dA + dF + dW |
|
15 AFWWFA = rho * sp.convolve(AFW, AFW[::-1]) |
|
16 BV = sp.convolve(B, V) |
|
17 dBV = dB + dV |
|
18 BVVB = sp.convolve(BV, BV[::-1]) |
|
19 diff = dAFW - dBV |
|
20 dBVVB = 2*dBV |
|
21 for i in range(diff): |
|
22 BVVB, dBVVB = polmul(BVVB, dBVVB, sp.array([0, 1]), 1) |
|
23 rbb, drbb = poladd(AFWWFA, 2*dAFW, BVVB, dBVVB) |
|
24 rbb = rbb.squeeze() |
|
25 rts = sp.roots(rbb) |
|
26 rtsin = rts[dAFW:2*dAFW+1] |
|
27 b = 1 |
|
28 for i in range(dAFW): |
|
29 b = sp.convolve(b, sp.array([1, -rtsin[i]])) |
|
30 b = sp.real(b) |
|
31 bbr = sp.convolve(b, b[::-1]) |
|
32 r = rbb[0] / bbr[0] |
|
33 return r, b, dAFW |
|
34 |
|
35 if __name__ == "__main__": |
|
36 A, dA = sp.array([1, -0.44]), 1 |
|
37 B, dB = sp.array([0.51, 1.21]), 1 |
|
38 F, dF = sp.array([1, -1]), 1 |
|
39 V, W = 1, 1 |
|
40 dV, dW = 0, 0 |
|
41 rho = 1 |
|
42 int1 = 1 |
|
43 print specfac(A,dA,B,dB,rho,V,dV,W,dW,F,dF) |
|
44 |