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