|
1 #!/usr/bin/python |
|
2 |
|
3 import pylab as pl |
|
4 from scipy.linalg import schur |
|
5 from dscr import dscr |
|
6 from scipy import signal |
|
7 |
|
8 def dlyap(a, b): |
|
9 n = len(a) |
|
10 x = pl.zeros_like(a) |
|
11 s, u = schur(a) |
|
12 b = pl.dot(u.T, pl.dot(b,u)) |
|
13 j = n-1 |
|
14 while j>=0: |
|
15 k = j |
|
16 ## Check for Schur block. |
|
17 if j==0: |
|
18 blksiz = 1 |
|
19 elif s[j, j-1]!=0: |
|
20 blksiz = 2 |
|
21 j = j - 1 |
|
22 else: |
|
23 blksiz = 1 |
|
24 Ajj = pl.kron(s[j:k+1,j:k+1], s) - pl.eye(blksiz*n) |
|
25 rhs = pl.reshape(b[:,j:k+1].T, (blksiz*n, 1)) |
|
26 if (k < n-1): |
|
27 rhs2 = pl.dot(s, pl.dot(x[:,k+1:n], s[j:k+1, k+1:n].T)) |
|
28 rhs = rhs + pl.reshape(rhs2, (blksiz*n, 1)) |
|
29 v = -pl.solve(Ajj, rhs) |
|
30 x[:,j] = v.squeeze()[:n] |
|
31 if(blksiz == 2): |
|
32 x[:, k] = v[n:blksiz*n].squeeze() |
|
33 j = j - 1 |
|
34 |
|
35 ## Back-transform to original coordinates. |
|
36 x = pl.dot(u, pl.dot(x, u.T)) |
|
37 return x |
|
38 |
|
39 |
|
40 def covar_m(H, W): |
|
41 """ |
|
42 User defined equivalent function to Matlab covar function |
|
43 For discrete time domain only |
|
44 Uses Lyapunov's equation for computation |
|
45 W: noise intensity (scalar) |
|
46 """ |
|
47 a = pl.roots(H.den) |
|
48 if pl.any(abs(a) > 1): |
|
49 # print "Warning: System being unstable has infinite covariance" |
|
50 P = pl.inf |
|
51 return P |
|
52 else: |
|
53 A, B, C, D = H.A, H.B, H.C, H.D |
|
54 # Sylvester and Lyapunov solver |
|
55 Q1 = pl.dot(-B, pl.dot(W, B.T)) |
|
56 Q = dlyap(A, -Q1) |
|
57 # Q = linmeq(2,A,Q1,[1, 0],1) |
|
58 # A*X*A' - X + B*W*B' = 0, (2b) |
|
59 # Discrete time Lyapunov equation; A is general form. Hessenberg-Schur method. |
|
60 # linmeq(2, A, C, [1,0], 1) |
|
61 # A*X*A' - X = C, (2b) |
|
62 # |
|
63 P = pl.dot(C, pl.dot(Q,C.T)) + pl.dot(D, pl.dot(W,D.T)) |
|
64 |
|
65 return P |