--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lqg/specfac.py	Fri May 27 14:24:59 2011 +0530
@@ -0,0 +1,44 @@
+#!/usr/bin/python
+# 13.3
+import os, sys
+sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"]
+
+import scipy as sp
+from polyfuncs import polmul, poladd
+
+def specfac(A, dA, B, dB, rho, V, dV, W, dW, F, dF):
+    """Implements the spectral factorization for use with LQG 
+    control by design method of Ahlen and Sternad"""
+
+    AFW = sp.convolve(A, sp.convolve(W, F))
+    dAFW = dA + dF + dW
+    AFWWFA = rho * sp.convolve(AFW, AFW[::-1])
+    BV = sp.convolve(B, V)
+    dBV = dB + dV
+    BVVB = sp.convolve(BV, BV[::-1])
+    diff = dAFW - dBV
+    dBVVB = 2*dBV
+    for i in range(diff):
+        BVVB, dBVVB = polmul(BVVB, dBVVB, sp.array([0, 1]), 1)
+    rbb, drbb = poladd(AFWWFA, 2*dAFW, BVVB, dBVVB)
+    rbb = rbb.squeeze()
+    rts = sp.roots(rbb)
+    rtsin = rts[dAFW:2*dAFW+1]
+    b = 1
+    for i in range(dAFW):
+        b = sp.convolve(b, sp.array([1, -rtsin[i]]))
+    b = sp.real(b)
+    bbr = sp.convolve(b, b[::-1])
+    r = rbb[0] / bbr[0]
+    return r, b, dAFW
+
+if __name__ == "__main__":
+    A, dA = sp.array([1, -0.44]), 1
+    B, dB = sp.array([0.51, 1.21]), 1
+    F, dF = sp.array([1, -1]), 1
+    V, W = 1, 1
+    dV, dW = 0, 0
+    rho = 1
+    int1 = 1
+    print specfac(A,dA,B,dB,rho,V,dV,W,dW,F,dF)
+