Initial commit.
#!/usr/bin/python
# 14.1
# Uses A, B from 2.1 (pend_model.py)
import os, sys
sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"]
import scipy as sp
from scipy import signal
from scipy import linalg
from dscr import dscr
from pend_model import pend_model
def pol2cart(theta, r):
x = r*sp.cos(theta)
y = r*sp.sin(theta)
return x, y
def cont_mat(a, b):
n = len(a)
con = b
for i in range(1, n):
prod = sp.identity(n)
for j in range(i):
prod = sp.dot(prod, a)
prod = sp.dot(prod, b)
con = sp.hstack((con, prod))
return con
def delta_a(a, P):
n = len(a)
delta_a = sp.identity(n)
for pole in P:
delta_a = sp.dot(delta_a, (a - pole*sp.identity(n)))
return delta_a
def acker(a, b, P):
""" Crude implementation of ackermann's formula """
con = cont_mat(a, b)
con_inv = linalg.inv(con)
del_a = delta_a(a, P)
e_n = sp.zeros(len(a))
e_n[-1] = 1
K = sp.dot(sp.dot(e_n, con_inv), del_a)
return K
if __name__ == "__main__":
A, B = pend_model()
# Changed form of C, D from the scilab one to make it work with signal.lti
C = sp.eye(1, 4)
D = 0
Ts = 0.01
G = signal.lti(A, B, C, D)
H = dscr(G, Ts)
a, b, c, d = H.A, H.B, H.C, H.D
rise = 5
epsilon = 0.1
N = rise/Ts
omega = sp.pi/2/N
r = epsilon**(omega/sp.pi)
r1 = r
r2 = 0.9*r
x1, y1 = pol2cart(omega, r1)
x2, y2 = pol2cart(omega, r2)
p1 = x1+y1*1j
p2 = x2+y2*1j
p3 = x1-y1*1j
p4 = x2-y2*1j
P = [[p1], [p2], [p3], [p4]]
K = acker(a,b,P)
print K