In [ ]:
import numpy as np
import sympy as sp
from sympy import init_printing
from sympy import pprint as show
from __future__ import division
from scipy.optimize import minimize
In [ ]:
def sat(v, u_max):
return np.clip(v, -u_max, u_max)
#Test
#print sat([2,-3,4,1], 2)
#print sat([[2, 3],[-3, -4],[4, 0],[1, 0.5]], 3)
In [ ]:
# uboot
A = np.array([[0., 1., 0.],
[0., 0., 1.],
[0., 0., -0.005]])
#last line of Regelungsnormalform (characteristical polynom coefficients) in a colunm vector
a = -np.array(A[np.array(A).shape[1]-1])[np.newaxis].T
b = np.array([[0], [0], [1.]])
d = 0
c = np.array([[1], [0], [0]])
u_max = 2.5e-5
#simulation time
T = 1.0/200.0
#Introduced for fun
u_max_sys = u_max
In [ ]:
# already Results
a_hat = np.array([[4.4469e-8], [2.3073e-5], [4.9148e-3]])
a_hat_star = np.array([[1.073e-7], [4.919e-5], [10.4078e-3]])
R1 = np.array([[1.6021e-5, 3.26098e-3, 0.4031],
[3.2698e-3, 1.5666, 163.46],
[0.4031, 163.46, 40.713]])
#show(sp.Matrix(R1))
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])
# print "p:", p
# print "D_inv:", D_inv
# print "x_T:", x.transpose()
# print "x:", x
# print "R1:", R1
g = x.transpose().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 [ ]:
import time
class Timer(object):
def __init__(self, name=None):
self.name = name
def __enter__(self):
self.tstart = time.time()
def __exit__(self, type, value, traceback):
if self.name:
print '[%s]' % self.name,
print 'Elapsed: %s' % (time.time() - self.tstart)
In [ ]:
# Defining symbolic representation (optional to above)
p = sp.symbols('p', positive=True)
D, R = sp.symbols('D, R')
D = sp.diag(p**3, p**2, p)
#print D**-1
R = D**-1 * sp.Matrix(R1) * D**-1
#show(R)
x, x1, x2, x3 = sp.symbols('x x1 x2 x3')
x = sp.Matrix([[x1], [x2], [x3]])
#show(x)
g = sp.symbols('g')
#g = sp.Poly((x.T * R * x - sp.Matrix([1.0])), p)
g = x.T * R * x - sp.Matrix([1.0])
#show(g)
#g.all_coeffs()
k_star = sp.symbols('k_star')
k_star = D**-1 * sp.Matrix(a_hat_star) - sp.Matrix(a)
#k_star
#show(g.all_coeffs())
In [ ]:
from IPython.display import clear_output
# Numerical approach
pmin = 0.01
# Initial state
x0 = np.array([[0],[0],[-0.004]])
# Timeloop
x_t = x0
# Make functionpointer
func_g = lambda p: np.absolute(get_g(p, x_t, R1))
func_kstar = lambda p: get_kstar(p, a, a_hat_star)
init_p = 0.0255226957823816
max_T = 1000 # Seconds
max_iter = int(max_T/T)
y_t = np.zeros(max_iter)
p_t = np.zeros(max_iter)
u_t = np.zeros(max_iter)
u_t2 = np.zeros(max_iter)
with Timer():
for t in range(0, max_iter):
#for t in range(0, 10000):
if t%1000 == 1:
clear_output()
print t*T, "seconds done ->", t/max_iter*100, "%"
## Calc p
res = minimize(func_g, p_t[t], method='Nelder-Mead')
#print res.x
#g_poly = sp.Poly(sys, p)
#show(g_poly)
if res.x < pmin:
p = pmin
else:
p = res.x
p_t[t] = p
## Calc u
k_t = func_kstar(p)
u = np.dot(np.array(-k_t.T), x_t)
#print u_sym
u_t[t] = u
u = sat(u, u_max)
u_t2[t] = u
## System response
# System saturation u (trivial if u_max and u_max_sys are identical)
# Calc x_dot
x_dot = np.dot(A, x_t) + b * sat(u, u_max_sys)
x_t = x_t + x_dot*T
y_t[t] = x_t[0]
clear_output()
print "done"
In [ ]:
print func_kstar(0.4)
In [ ]:
# Symbolic approach
def run_sym_approach():
# Initial state
x0 = np.array([[0],[0],[-0.004]])
# Timeloop
x_t = x0
y_t = x0[0]
for t in range(1, 30):
## Calc p
sys = g.subs({x1:x_t[0][0],
x2:x_t[1][0],
x3:x_t[2][0]})
#g_poly = sp.Poly(sys, p)
#show(g_poly)
p_t = sp.solve(sys, p)
if len(p_t) < 1:
print "No solution found. Timestep:", t
print g
print sys
#print p_t
break
else:
print "p:", p_t
## Calc u
k_t = k_star.subs(p, p_t[0][0])
#print np.array(k_t.T)
u = np.dot(np.array(-k_t.T), x_t)
#print u_sym
u = sat(u_sym[0], u_max)
## System response
# System saturation u (trivial if u_max and u_max_sys are identical)
# Calc x_dot
#show(A)
#show(x_t)
x_dot = np.dot(A, x_t) + b * sat(u, u_max_sys)
#print x_dot*T
#print x_t
x_t = x_t + x_dot*T
#print x_dot
y_t = np.append(y_t, x_t[0])
#print y_t
#sys
#x_t[2][0]
return y_t, x_t
In [ ]:
import matplotlib.pyplot as plt
plt.rcParams['figure.figsize'] = (15, 20)
plt.subplot(311)
plt.plot(np.arange(0, len(y_t))*T, y_t, 'b')
plt.xlabel('t [s]')
plt.ylabel('y(t)')
plt.subplot(312)
plt.plot(np.arange(0, len(y_t))*T, p_t, 'g')
axes = plt.gca()
axes.set_ylim([0, 1.1])
plt.xlabel('t [s]')
plt.ylabel('p(t)')
plt.subplot(313)
axes = plt.gca()
axes.set_ylim([-u_max-1e-5,u_max+1e-5])
plt.plot(np.arange(0, len(y_t))*T, u_t, 'r--')
plt.plot(np.arange(0, len(y_t))*T, u_t2, 'r')
plt.xlabel('t [s]')
plt.ylabel('u(t)')
plt.show()
In [ ]: