1 #!/usr/bin/env python

     2 # 12.11

     3 

     4 import os, sys

     5 sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"]

     6 

     7 import pylab as pl

     8 from xdync import xdync

     9 from zpowk import zpowk

    10 from polyfuncs import polmul, poladd

    11 

    12 def gpc_pid(A, dA, B, dB, C, dC, N1, N2, Nu, lambda1, gamm, gamma_y):

    13     Adelta = pl.convolve(A,[1, -1])

    14     G = pl.array([])

    15     Gtilda1 = pl.array([])

    16     for i in range(N1, N2+1):

    17         zi=zpowk(i)[0]

    18         E, dE, F, dF = xdync(Adelta, dA+1, zi, i, C, dC)[:4]

    19         Gtilda, dGtilda, Gbar, dGbar = xdync(C, dC, zi, i, E*B, dE+dB)[:4]

    20         Gtilda = pl.atleast_1d(Gtilda.squeeze())

    21         Gtilda1 = pl.empty(i)

    22         for j in range(i): 

    23             Gtilda1[j] = Gtilda[i-j-1]

    24         if i <= Nu-1:

    25             Gtilda2 = pl.zeros(Nu)

    26             Gtilda2[:i] = Gtilda1

    27             if pl.size(G) is not 0:

    28                 G = pl.vstack((G, Gtilda2))

    29             else:

    30                 G = Gtilda2

    31         else:

    32             G = pl.vstack((G, Gtilda1[:Nu]))

    33 

    34     es=sum(pl.atleast_1d(C))/sum(pl.atleast_1d(A))

    35     gs=sum(pl.atleast_1d(B))/sum(pl.atleast_1d(A))

    36     F_s=es * A

    37     G_s=[]

    38     

    39     for i in range(1, Nu+1):

    40         if (Nu-i) == 0:

    41             row= gs * pl.ones(i)

    42         else:

    43             row = gs * pl.ones(i)

    44             row = pl.column_stack((row, pl.zeros((Nu-i,Nu-i))))

    45             row = row.squeeze()

    46         if pl.size(G_s) is not 0:

    47             G_s = pl.row_stack((G_s, row))

    48         else:

    49             G_s = row

    50 

    51     lambda_mat = lambda1 * pl.identity(Nu)

    52     gamma_mat = gamm * pl.identity(Nu)

    53     gamma_y_mat = gamma_y * pl.identity(N2-N1+1)

    54     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)))

    55     mat2 = pl.dot(mat1, pl.dot(G.T, gamma_y_mat))

    56     mat2_s = pl.dot(mat1, pl.dot(G_s.T, gamma_mat))

    57     h_s= sum(mat2_s[0,:])

    58     h = mat2[0,:]

    59     T = C 

    60     R = C * (sum(h)+h_s)

    61     S = 0

    62     for i in range (N1, N2+1):

    63         zi=zpowk(i)[0]

    64         E, dE, F, dF = xdync(Adelta, dA+1, zi, i, C, dC)[:4]

    65         Gtilda, dGtilda, Gbar, dGbar = xdync(C, dC, zi, i, E*B, dE+dB)[:4]

    66         S += F*h[i-1]

    67     S += F_s*h_s

    68     S = S.squeeze()

    69     if len(A) == 3:

    70         Kp = S[0] - R - S[2]

    71         Ki = R 

    72         Kd = S[2]

    73     else:

    74         Kp = S[1] - R

    75         Ki = R 

    76         Kd = 0

    77     return Kp, Ki, Kd

    78