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