--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pid/gpc_pid.py	Fri May 27 14:24:59 2011 +0530
@@ -0,0 +1,78 @@
+#!/usr/bin/env python
+# 12.11
+
+import os, sys
+sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"]
+
+import pylab as pl
+from xdync import xdync
+from zpowk import zpowk
+from polyfuncs import polmul, poladd
+
+def gpc_pid(A, dA, B, dB, C, dC, N1, N2, Nu, lambda1, gamm, gamma_y):
+    Adelta = pl.convolve(A,[1, -1])
+    G = pl.array([])
+    Gtilda1 = pl.array([])
+    for i in range(N1, N2+1):
+        zi=zpowk(i)[0]
+        E, dE, F, dF = xdync(Adelta, dA+1, zi, i, C, dC)[:4]
+        Gtilda, dGtilda, Gbar, dGbar = xdync(C, dC, zi, i, E*B, dE+dB)[:4]
+        Gtilda = pl.atleast_1d(Gtilda.squeeze())
+        Gtilda1 = pl.empty(i)
+        for j in range(i): 
+            Gtilda1[j] = Gtilda[i-j-1]
+        if i <= Nu-1:
+            Gtilda2 = pl.zeros(Nu)
+            Gtilda2[:i] = Gtilda1
+            if pl.size(G) is not 0:
+                G = pl.vstack((G, Gtilda2))
+            else:
+                G = Gtilda2
+        else:
+            G = pl.vstack((G, Gtilda1[:Nu]))
+
+    es=sum(pl.atleast_1d(C))/sum(pl.atleast_1d(A))
+    gs=sum(pl.atleast_1d(B))/sum(pl.atleast_1d(A))
+    F_s=es * A
+    G_s=[]
+    
+    for i in range(1, Nu+1):
+        if (Nu-i) == 0:
+            row= gs * pl.ones(i)
+        else:
+            row = gs * pl.ones(i)
+            row = pl.column_stack((row, pl.zeros((Nu-i,Nu-i))))
+            row = row.squeeze()
+        if pl.size(G_s) is not 0:
+            G_s = pl.row_stack((G_s, row))
+        else:
+            G_s = row
+
+    lambda_mat = lambda1 * pl.identity(Nu)
+    gamma_mat = gamm * pl.identity(Nu)
+    gamma_y_mat = gamma_y * pl.identity(N2-N1+1)
+    mat1 = pl.inv(pl.dot(G.T, pl.dot(gamma_y_mat, G))+lambda_mat+pl.dot(G_s.T, pl.dot(gamma_mat,G_s)))
+    mat2 = pl.dot(mat1, pl.dot(G.T, gamma_y_mat))
+    mat2_s = pl.dot(mat1, pl.dot(G_s.T, gamma_mat))
+    h_s= sum(mat2_s[0,:])
+    h = mat2[0,:]
+    T = C 
+    R = C * (sum(h)+h_s)
+    S = 0
+    for i in range (N1, N2+1):
+        zi=zpowk(i)[0]
+        E, dE, F, dF = xdync(Adelta, dA+1, zi, i, C, dC)[:4]
+        Gtilda, dGtilda, Gbar, dGbar = xdync(C, dC, zi, i, E*B, dE+dB)[:4]
+        S += F*h[i-1]
+    S += F_s*h_s
+    S = S.squeeze()
+    if len(A) == 3:
+        Kp = S[0] - R - S[2]
+        Ki = R 
+        Kd = S[2]
+    else:
+        Kp = S[1] - R
+        Ki = R 
+        Kd = 0
+    return Kp, Ki, Kd
+