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