|
1 #!/usr/bin/python |
|
2 # 13.4 |
|
3 |
|
4 import os, sys |
|
5 sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"] |
|
6 |
|
7 import scipy as sp |
|
8 from polyfuncs import polmul, putin, ext |
|
9 from xdync import xdync |
|
10 from zpowk import zpowk |
|
11 from specfac import specfac |
|
12 |
|
13 def lqg_simple(A,dA,B,dB,C,dC,k,rho,V,dV,W,dW,F,dF): |
|
14 """ LQG controller design by method of Ahlen and Sternad. """ |
|
15 r, b, db = specfac(A,dA,B,dB,rho,V,dV,W,dW,F,dF) |
|
16 D, dD = polmul(A, dA, F, dF) |
|
17 zk, dzk = zpowk(k) |
|
18 N, dN = polmul(zk, dzk, B, dB) |
|
19 RHS, dRHS = polmul(C, dC, b, db) |
|
20 S, dS, R1, dR1 = xdync(N, dN, D, dD, RHS, dRHS)[:4] |
|
21 return R1, dR1, S, dS |
|
22 |
|
23 if __name__ == "__main__": |
|
24 A, dA = sp.array([1, -0.44]), 1 |
|
25 B, dB = sp.array([0.51, 1.21]), 1 |
|
26 F, dF = sp.array([1, -1]), 1 |
|
27 C, dC = sp.array([1, -0.44]),1 |
|
28 k = 1 |
|
29 V, W = 1, 1 |
|
30 dV, dW = 0, 0 |
|
31 rho = 1 |
|
32 int1 = 1 |
|
33 |
|
34 print lqg_simple(A,dA,B,dB,C,dC,k,rho,V,dV,W,dW,F,dF) |