| 6 | |
| 7 | |
| 8 | class TransferFunction: |
| 9 | def __init__(self, num, den, Ts): |
| 10 | # expects num and den to be numpy arrays of shape (1,m) and (1,n) |
| 11 | m = num.shape[1] |
| 12 | n = den.shape[1] |
| 13 | # set initial conditions |
| 14 | self.state = np.zeros((n-1, 1)) |
| 15 | self.Ts = Ts |
| 16 | # make the leading coef of den == 1 |
| 17 | if den.item(0) != 1: |
| 18 | den = den / den.item(0) |
| 19 | num = num / den.item(0) |
| 20 | self.num = num |
| 21 | self.den = den |
| 22 | # set up state space equations in control canonic form |
| 23 | self.A = np.zeros((n-1, n-1)) |
| 24 | self.B = np.zeros((n-1, 1)) |
| 25 | self.C = np.zeros((1, n-1)) |
| 26 | self.B[0][0] = 1.0 |
| 27 | if m == n: |
| 28 | self.D = num.item(0) |
| 29 | for i in range(0, n-1): |
| 30 | self.C[0][i] = num.item(i+1) - num.item(0)*den.item(i+1) |
| 31 | for i in range(0, n-1): |
| 32 | self.A[0][i] = - den.item(i+1) |
| 33 | for i in range(1, n-1): |
| 34 | self.A[i][i-1] = 1.0 |
| 35 | else: |
| 36 | self.D = 0.0 |
| 37 | for i in range(0, m): |
| 38 | self.C[0][n-i-2] = num.item(m-i-1) |
| 39 | for i in range(0, n-1): |
| 40 | self.A[0][i] = -den.item(i+1) |
| 41 | for i in range(1, n-1): |
| 42 | self.A[i][i-1] = 1.0 |
| 43 | # print("A=",self.A) |
| 44 | # print("B=",self.B) |
| 45 | # print("C=",self.C) |
| 46 | # print("D=",self.D) |
| 47 | |
| 48 | def update(self, u): |
| 49 | self.rk4_step(u) |
| 50 | y = self.h(u) |
| 51 | return y |
| 52 | |
| 53 | def f(self, state, u): |
| 54 | xdot = self.A @ state + self.B * u |
| 55 | return xdot |
| 56 | |
| 57 | def h(self, u): |
| 58 | y = self.C @ self.state + self.D * u |
| 59 | return y.item(0) |
| 60 | |
| 61 | def rk4_step(self, u): |
| 62 | # Integrate ODE using Runge-Kutta 4 algorithm |
| 63 | F1 = self.f(self.state, u) |
| 64 | F2 = self.f(self.state + self.Ts / 2 * F1, u) |
| 65 | F3 = self.f(self.state + self.Ts / 2 * F2, u) |