In [ ]:

    

from pprint import pprint as pprint
import numpy as np
import matplotlib.pyplot as plt

import control as pc

%pylab inline

import pandas as pd

from sysident import loadtools

import scipy as sp
import numpy.linalg as la

import sympy


    

In [ ]:

    




    

In [ ]:

    

folder = "/home/lth/jupyter_nb/optimization/models/"
#fname = folder+"ss1_20180724-082630_poles2_ident_pade1_0:036_control_20180724-092309.npy"
#fname = folder+"ss2_20180724-082631_poles3_ident_pade2_0:036_control_20180724-092409.npy"
#fname = folder+"ss3_position_20180717-104106_poles3_ident_pade1_0:032_control_20180724-092445.npy"
fname = folder+"ss5_20180724-082628_poles3_ident_pade1_0:032_control_20180724-092522.npy"
#fname = folder+"ss6_20180724-082629_poles4_ident_pade2_0:032_control_20180724-092618.npy"

res, _ = loadtools.loadNPY(fname)
pprint(res.keys())


    

In [ ]:

    

plt.plot(np.linspace(-10, 10), np.power(np.linspace(-10, 10), 6))


    

In [ ]:

    

sp.special.pseudo_huber(0.1, [inf, inf]).cumsum()


    

In [ ]:

    

# Creates a PT2+delay system from parameters
def get_systems(K, T, D, Td, Td_o=1):
    sys0 = pc.tf([K],[T**2, 2*D*T, 1])
    sys = pc.series(pc.tf(*pc.pade(Td, Td_o)), sys0)
    return sys0, sys


    

In [ ]:

    

from scipy.optimize import minimize

pole_limit = -80 

# Optimization function:
# creates PT2-d system, transforms to modal form,
# recreates A with idealized A (eye*eigval)
# ma
def fun(arg, Td_o=1, show=False):
    
    K, D, T, Td = arg
    #print K, D, T, Td
    #T = np.abs(T)
    
    sys0, sys = get_systems(K, T, D, Td, Td_o)
    
    ss = pc.tf2ss(sys)
    
    eigval, Tzx = la.eig(ss.A)
    A_modal = la.solve(Tzx, ss.A).dot(Tzx)
    B_modal = la.solve(Tzx, ss.B)
    C_modal = ss.C.dot(Tzx)
    D_modal = ss.D
    
    t, y = pc.step_response(pc.ss(np.eye(3)*eigval, B_modal, C_modal, D_modal))
    
    df1 = pd.DataFrame(res['plot_input_step'][1], index=res['plot_input_step'][0])
    df2 = pd.DataFrame(y, index=t).fillna(np.inf)
    #print df2
    df3 = pd.concat([df1, df2], axis=1).interpolate()
    
    df_diff = df3.diff(axis=1).iloc[:,1]
    #print np.array(df_diff.cumsum().tail(), dtype=float64)
    
    poles = pc.pole(pc.ss(np.eye(3)*eigval, B_modal, C_modal, D_modal))
    #print poles[poles < -pole_limit]
    #print poles.min()
    p = np.sum(np.power(poles[poles < pole_limit], 10))
    #print poles
    #print p
    r = sp.special.pseudo_huber(0.2, np.array(df_diff, dtype=float64)).cumsum()[-1]
    
    #r = np.array(df3.diff(axis=1).abs().cumsum())[-1][1]
    
    if np.isnan(r):
        r = np.inf
    
    if show:
        plt.plot(t, y)
        plt.plot(*res['plot_input_step'])
        plt.show()

        df_diff.plot()
        print r+p, '>>', arg
    return r+p

#fun([1, 1, 0.1, 0.1], show=True)


    

In [ ]:

    

result = minimize(fun, [1, 1, 0.1, 0.1], method='Powell')
fun(result.x, show=True)

K, D, T, Td = result.x
#Td_o = 1 !!!!!!!!!!!!!!!!
sys0, sys = get_systems(K, T, D, Td, Td_o=1)
ss = pc.tf2ss(sys)
eigval, Tzx = la.eig(ss.A)
A_modal = la.solve(Tzx, ss.A).dot(Tzx)
B_modal = la.solve(Tzx, ss.B)
C_modal = ss.C.dot(Tzx)
D_modal = ss.D

print pc.pole(sys)


    

In [ ]:

    

result = minimize(fun, [1, 1, 0.1, 0.1], method='Nelder-Mead')
fun(result.x, show=True)

K, D, T, Td = result.x
#Td_o = 1 !!!!!!!!!!!!!!!!
sys0, sys = get_systems(K, T, D, Td, Td_o=1)
ss = pc.tf2ss(sys)
eigval, Tzx = la.eig(ss.A)
A_modal = la.solve(Tzx, ss.A).dot(Tzx)
B_modal = la.solve(Tzx, ss.B)
C_modal = ss.C.dot(Tzx)
D_modal = ss.D

print pc.pole(sys)


    

In [ ]:

    

df1 = pd.DataFrame(res['plot_input_step'][1], index=res['plot_input_step'][0])
df2 = pd.DataFrame(res['plot_model_step'][1], index=res['plot_model_step'][0])

df3 = pd.concat([df1, df2], axis=1).interpolate()

plt.plot(*res['plot_model_step'])
plt.plot(*res['plot_input_step'])
plt.show()

df3.diff(axis=1).plot()
r = np.array(df3.diff(axis=1).abs().cumsum())[-1][1]
print r


    

In [ ]:

    

K, D, T, Td = result.x

#Td_o = 1 !!!!!!!!!!!!!!!!
sys0, sys = get_systems(K, T, D, Td)
ss = pc.tf2ss(sys)


    

In [ ]:

    

eigval, Tzx = la.eig(ss.A)
print eigval
print Tzx


    

In [ ]:

    

A_modal = la.solve(Tzx, ss.A).dot(Tzx)
B_modal = la.solve(Tzx, ss.B)
C_modal = ss.C.dot(Tzx)
D_modal = ss.D


    

In [ ]:

    

print A_modal
print np.eye(3)*eigval

print np.eye(3)*eigval - A_modal


    

In [ ]:

    

plt.plot(*pc.step_response(ss))
plt.plot(*pc.step_response(pc.ss(A_modal, B_modal, C_modal, D_modal)))
plt.plot(*pc.step_response(pc.ss(np.eye(3)*eigval, B_modal, C_modal, D_modal)))


    

In [ ]:

    

fname = 'ss_{}_poles{}_manual_ident_huber{}'.format(time.strftime("%Y%m%d-%H%M%S"),
                                    len(pc.pole(sys)), 'pade{}_{}'.format(Td_o, Td))


loadtools.saveDelayModel(fname, ss.A, ss.B, ss.C, ss.D, Td)


    

In [ ]: