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