--- /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)
+