|
1 #!/usr/bin/env python |
|
2 # 12.2 |
|
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 polyfuncs import polmul, poladd |
|
10 |
|
11 def gpc_Nc(A, dA, B, dB, C, dC, k, N1, N2, Nu, rho): |
|
12 D, dD = pl.array([1, -1]), 1 |
|
13 AD, dAD = pl.convolve(A, D), dA+1 |
|
14 zj, dzj = 1, 0 |
|
15 for i in range(N1+k-1): |
|
16 zj = sp.convolve(zj, [0,1]) |
|
17 dzj += 1 |
|
18 M = 2*k+N2-2+dB |
|
19 P = max(k+N2+dA-1, dC-1) |
|
20 G = pl.zeros((N2-N1+1, Nu+1)) |
|
21 H1 = pl.zeros((N2-N1+1, M)) |
|
22 H2 = pl.zeros((N2-N1+1, P+1)) |
|
23 |
|
24 for j in range(k+N1, k+N2+1): |
|
25 zj = pl.convolve(zj, [0,1]) |
|
26 dzj = dzj + 1 |
|
27 Fj, dFj, Ej, dEj = xdync(zj, dzj, AD, dAD, C, dC)[:4] |
|
28 Nj, dNj, Mj, dMj = xdync(zj, dzj, C, dC, 1, 0)[:4] |
|
29 Gj, dGj = polmul(Mj, dMj, Ej, dEj) |
|
30 Gj, dGj = polmul(Gj, dGj, B, dB) |
|
31 Pj, dPj = polmul(Mj, dMj, Fj, dFj) |
|
32 Pj, dPj = poladd(Nj, dNj, Pj, dPj) |
|
33 if j-k >= Nu: |
|
34 G[j-(k+N1),:Nu+1] = Gj[range(j-k, j-k-Nu-1, -1)] |
|
35 else: |
|
36 G[j-(k+N1),:j-k+1] = Gj[j-k::-1] |
|
37 H1[j-(k+N1),:j+k-2+dB] = Gj[j-k+1:2*j+dB-1] |
|
38 dPj = max(j-1+dA, dC-1) |
|
39 H2[j-(k+N1),:dPj+1] = Pj |
|
40 K = pl.dot(pl.inv(pl.dot(G.T, G) + rho*pl.eye(Nu+1, Nu+1)), G.T) |
|
41 KH1 = pl.dot(K, H1) |
|
42 KH2 = pl.dot(K, H2) |
|
43 R1 = pl.concatenate(([1], KH1[0,:])) |
|
44 dR1 = len(R1)-1 |
|
45 Sc = KH2[0,:] |
|
46 dSc = len(Sc)-1; |
|
47 Tc = K[0,:] |
|
48 dTc = len(Tc)-1; |
|
49 |
|
50 return K, KH1, KH2, Tc, dTc, Sc, dSc, R1, dR1 |