--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lqg/lqg.py	Fri May 27 14:24:59 2011 +0530
@@ -0,0 +1,61 @@
+#!/usr/bin/python
+# 13.4
+
+import os, sys
+sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"]
+
+import scipy as sp
+from polyfuncs import polmul, putin, ext
+from xdync import xdync
+from specfac import specfac
+
+def lqg(A,degA,B,degB,C,degC,k,rho,V,degV,W,degW,F,degF): 
+    """ LQG controller design by method of Ahlen and Sternad. """
+    r, b, degb = specfac(A,degA,B,degB,rho,V,degV,W,degW,F,degF)
+    print r, b, degb, "specfac"
+    WFA = sp.convolve(A, sp.convolve(F,W))[::-1]
+    dWFA = degW + degF + degA
+    rhs1, drhs1 = polmul(W, degW, WFA, dWFA)
+    rhs1, drhs1 = polmul(rhs1, drhs1, C, degC)
+    rhs1 = rho * rhs1
+    rhs2 = sp.convolve(C, sp.convolve(V, sp.convolve(B,V)[::-1]))
+    drhs2 = degC + 2*degV + degB
+    for i in range(degb - degB - degV):
+        rhs2 = sp.convolve(rhs2, [0,1])
+    drhs2 = drhs2 + degb - degB - degV
+    C1 = sp.zeros((1,2))
+
+    C1, degC1 = putin(C1,0,rhs1,drhs1,0,0)
+    C1, degC1 = putin(C1,degC1,rhs2,drhs2,0,1)
+    rbf = r * b[::-1]
+    D1 = sp.zeros((2, 2))
+    D1, degD1 = putin(D1,0,rbf,degb,0,0)
+    for i in range(k):
+        rbf = sp.convolve(rbf, [0, 1])
+    D1, degD1 = putin(D1,degD1,rbf,degb+k,1,1)
+    N = sp.zeros((1, 2))
+    N, degN = putin(N,0,-B,degB,0,0)
+    
+    AF, dAF = polmul(A,degA,F,degF)
+    N, degN = putin(N,degN,AF,dAF,0,1)
+    print N, degN, "N, degN"
+    Y, degY, X, degX = xdync(N, degN, D1, degD1, C1, degC1)[:4]
+
+    R, degR = ext(X,degX,0,0)
+    S, degS = ext(X,degX,0,1)
+    X = Y[::-1]
+
+    return R, degR, S, degS
+
+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
+    C, dC = sp.array([1, -0.44]),1
+    k = 1
+    V, W = 1, 1
+    dV, dW = 0, 0
+    rho = 1
+    int1 = 1
+
+    print lqg(A,dA,B,dB,C,dC,k,rho,V,dV,W,dW,F,dF)