In [ ]:
import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import minimize
In [ ]:
def sat(v, u_max):
return np.clip(v, -u_max, u_max)
def simulate(A, B, C, D, regulator_func, s, T, umax=None, x0=0):
#intitialize y, u
y = np.matrix(np.zeros((C.shape[0],len(T))))
u = np.zeros((len(T),np.size(x0,1)))
u_sat = np.zeros((len(T),np.size(x0,1)))
if type(x0) is int:
xt = np.matrix([x0]*len(A)).T
print "x0 = \n{}".format(xt)
else:
xt = x0
#print "y.shape = \n",y.shape
#print "len(T) = \n",len(T)
#print "A.shape = \n",(A).shape
#print "xt.shape = \n",(xt).shape
#print "B.shape = \n",(B).shape
#print "u.shape = \n",u.shape
#print "s.shape = \n",s.shape
#print "C.T.shape = \n",C.T.shape
#print "D.shape = \n",D.shape
for i, t in enumerate(T):
u[[i],:] = regulator_func(y[:,i], s[i], xt)
if umax is not None:
u_sat[[i],:] = sat(u[[i],:], umax)
else:
u_sat[[i],:] = u[[i],:]
x_dot = A.dot(xt) + B.dot(u_sat[[i],:])
y[:,i] = C.dot(xt) + D.dot(u_sat[[i],:])
#print "u[[i],:].shape = \n",u[[i],:].shape
#print "xt = \n",xt
#print "regulator_func = \n",(regulator_func(y[i], s[i],xt))
#print "x_dot = \n",x_dot
#print "(C.T).dot(xt) = \n",((C.T).dot(xt)).shape
#print " D.dot(u[[i],:]) = \n",(D.dot(u[[i],:])).shape
#print "(C.T).dot(xt) + D.dot(u[[i],:]) = \n",((C.T).dot(xt) + D.dot(u[[i],:])).shape
#print "y[[i]] = \n",y[[i]]
if i < len(T)-1:
xt = xt + x_dot*(T[i+1]-T[i])
return y, u, u_sat
In [ ]:
# state-based simulation equation:
# x_dot = A.x + B.u
# y = C.x + D.u
# e.g., u = -(k.T).x. u can be a non-linear
# ref: Inverted Pendulum: System Modeling
# http://ctms.engin.umich.edu/CTMS/index.php?example=InvertedPendulum§ion=ControlStateSpace
# test a Inverted Pendulum function
M = 0.5;
m = 0.2;
b = 0.1;
I = 0.006;
g = 9.8;
l = 0.3;
p = I*(M+m)+M*m*(l**2); #denominator for the A and B matrices
# State space matries: A, B, C, D
A = np.matrix([[0 ,1 ,0 ,0],
[0 ,-(I+m*(l**2))*b/p ,(m**2)*g*(l**2)/p ,0],
[0 ,0 ,0 ,1],
[0 ,-(m*l*b)/p ,m*g*l*(M+m)/p ,0]]);
B = np.matrix([[ 0],
[(I+m*(l**2))/p],
[0],
[m*l/p],]);
C = np.matrix([[1 ,0 ,0 ,0],
[0 ,0 ,1 ,0],]);
D = np.matrix([[0],
[0]]);
# initial X
x0 = np.matrix(np.zeros(A.shape[0])).T
#T: time
T = np.arange(0, 5, 0.01)
#s: input, e.g., step function with amplitude of 0.2
s = 0.2*np.ones(len(T));
In [ ]:
# Regulator function
def closedLoop(y, s, x):
# fill-in K matrix euation. Below is just a controller matrix for
# the Inverted Pendulum pendulum problem
K = np.array([-70.7107 ,-37.8345 ,105.5298 ,20.9238])
return s-K.dot(x)
In [ ]:
# opened loop
inputSignal = lambda y,s,x: s
regulator_func = inputSignal
y, u, u_sat = simulate(A, B, C, D, regulator_func, s, T, x0=x0)
plt.figure()
line1, = plt.plot(T, np.array(y[0,:].T), 'b', label='cart position (m)')
line2, = plt.plot(T, np.array(y[1,:].T), 'g', label='pendulum angle (radians)')
first_legend = plt.legend(handles=[line1], loc=1)
plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.)
plt.xlabel('T')
plt.ylabel('y')
plt.title('Step Responce for Open Loop')
plt.show()
In [ ]:
# closed loop
regulator_func = closedLoop
y, u, u_sat= simulate(A, B, C, D, regulator_func, s, T, x0=x0)
plt.figure()
line1, = plt.plot(T, np.array(y[0,:].T), 'b', label='cart position (m)')
line2, = plt.plot(T, np.array(y[1,:].T), 'g', label='pendulum angle (radians)')
first_legend = plt.legend(handles=[line1], loc=1)
plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.)
plt.xlabel('T')
plt.ylabel('y')
plt.title('Step Responce for Closed Loop With Controller')
plt.show()
In [ ]:
# uboot
A = np.matrix([[0., 1., 0.],
[0., 0., 1.],
[0., 0., -0.005]])
#last line of Regelungsnormalform (characteristical polynom coefficients) in a colunm vector
a = -A[-1,:].T
B = np.matrix([[0], [0], [1.]])
C = np.matrix([1, 0, 0])
D = np.matrix([0]);
u_max = 2.5e-5
#T: time
T = np.arange(0, 900, 1)
#s: input, e.g., step function with amplitude of 0.2
s = np.zeros(len(T));
In [ ]:
# Optimation Results for reulator
a_hat = np.matrix([[4.4469e-8], [2.3073e-5], [4.9148e-3]])
a_hat_star = np.matrix([[1.073e-7], [4.919e-5], [10.4078e-3]])
R1 = np.matrix([[1.6021e-5, 3.26098e-3, 0.4031],
[3.2698e-3, 1.5666, 163.46],
[0.4031, 163.46, 40.713]])
In [ ]:
# Defining functions
def get_g(p, x, R1):
try:
p = p.squeeze() # This is weird
except:
pass
D_inv = np.diag([p**-3, p**-2, p**-1])
g = x.T.dot(D_inv).dot(R1).dot(D_inv).dot(x) - 1.0
# Update 2016: As of python 3.5, there is a new matrix_multiply symbol, @:
# g = x' @ D^-1 @ R1 @ D^-1 @ x - 1.0
return g.squeeze()
print "Test g\n", get_g(0.1, np.array([[1],[2],[3]]), R1)
def get_kstar(p, a, a_hat_star):
try:
p = p.squeeze() # This is weird
except:
pass
D_inv = np.diag([p**-3, p**-2, p**-1])
kstar = D_inv.dot(a_hat_star) - a
return kstar
print "Test kstar\n", get_kstar(2, a, a_hat_star)
In [ ]:
assert np.allclose(get_g(0.1, np.array([[1],[2],[3]]), R1), np.matrix(2086332.877))
assert np.allclose(get_kstar(2, a, a_hat_star), np.matrix([[ 1.34125000e-08], [ 1.22975000e-05], [ 2.03900000e-04]]))
In [ ]:
func_kstar = lambda p: get_kstar(p, a, a_hat_star)
assert np.allclose(func_kstar(0.4), np.matrix([[ 1.67656250e-06], [ 3.07437500e-04], [ 2.10195000e-02]]))
In [ ]:
init_p = 0.0255226957823816
pmin = 0.01
# Initial state
x0 = np.array([[0],[0],[-0.004]])
p_t = np.zeros(len(T))
In [ ]:
def contr_func(y, s, x):
# fill-in K matrix euation.
## Calc p
#print "x:",x
func_g = lambda p: np.absolute(get_g(p, x, R1))
res = minimize(func_g, pmin, method='Nelder-Mead')
# Saturate if too small
if res.x < pmin:
p = pmin
else:
p = res.x
#p_t[t] = p
#print "p:", p
## Calc K according to p
K = func_kstar(p).T
#print "K:", K
#print "K.shape:", K.shape
#print "s:", s
# Calc u
u = s-K.dot(x)
# Saturate u
u = sat(u, u_max)
#print "u", u
return u
In [ ]:
y, u, u_sat = simulate(A, B, C, D, contr_func, s, T, u_max, x0)
In [ ]:
y2, u2, u2_sat = simulate(A, B, C, D, lambda y, s, x: s-func_kstar(0.4).T.dot(x), s, T, u_max, x0)
In [ ]:
plt.figure()
line1, = plt.plot(T, np.array(y[0,:].T), 'b', label='y')
line2, = plt.plot(T, np.array(y2[0,:].T), 'r-', label='y2')
first_legend = plt.legend(handles=[line1], loc=1)
plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.)
plt.xlabel('T')
plt.ylabel('y')
plt.title('Step Response for Closed Loop With Controller')
plt.show()
In [ ]:
plt.figure()
line1, = plt.plot(T, u_sat, 'b', label='u')
line2, = plt.plot(T, u2_sat, 'r-', label='u fixed')
first_legend = plt.legend(handles=[line1, line2], loc=1)
plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.)
plt.xlabel('T')
plt.ylabel('y')
plt.title('Output values')
plt.show()
In [ ]:
In [ ]: