In [ ]:
import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import curve_fit
%pylab inline
In [ ]:
def func(x, a, b, c, d):
res=np.array([])
for x_single in x:
if x_single <= d:
res= np.append(res, [c])
else:
res = np.append(res, [a *(1 - np.exp(-b * (x_single-d))) + c])
return res
In [ ]:
#Wikipedia
def tn_pt2(xdata, td, k, w0, d):
raise(NotImplementedError('This function does not work as expected, some mathematical error occures! use pt2 instead'))
def _a1_2(w0, d):
#print 'd_w0:',d,w0
return (-d * w0 + w0 * np.sqrt(np.abs(d**2 - 1)),
-d * w0 - w0 * np.sqrt(np.abs(d**2 - 1)))
def _T1_2(a1_2):
#print a1_2
a1, a2 = a1_2
return (-1.0/a1, -1.0/a2)
def T1_2(w0, d):
return _T1_2(_a1_2(w0, d))
res = np.array([])
for t, x in xdata:
if t < td:
res = np.append(res, [0.0])
else:
t = t - td
#print 'x,td,t,d:',x, td, t,d
T1, T2 = T1_2(w0, d)
#print 'T1, T2:',T1, T2
if d > 1: # Kriechfall
val = k * (1 - (T1 / (T1 - T2)) * np.exp(-t / T1) + (T2 / (T1 - T2)) * np.exp(-t / T2)) * x
res = np.append(res, [val])
elif d == 1: #aperiodischer Grenzfall
#print "aperio"
#print 1.0/T1, np.exp(-t/T1)
val = k * ((1 - (1 + 1.0/T1 ) * np.exp(-t/T1))) * x
res = np.append(res, [val])
elif d <= -1: # instabiler Kriechfall
val = k * (1 - (T1 / (T1 - T2)) * np.exp(t / T1) + (T2 / (T1 - T2)) * np.exp(t / T2)) * x
res = np.append(res, [val])
else: #Schwingfall
#we = wo * np.sqrt(1-d**2)
val = k * (1 - 1/(np.sqrt(1-d**2)) * np.exp(-d*w0*t) * np.sin(w0*np.sqrt(1-d**2) * t + np.arccos(d))) * x
res = np.append(res, [val])
return res
In [ ]:
#t = np.arange(0, 10, 0.1)
#x = np.ones(len(t))
#tn_pt2(zip(t, x), td=0, k=2, w0=1, d=0.5)
In [ ]:
#t = np.arange(0, 70, 0.1)
#x = np.ones(len(t))
#xdata = zip(t, x)
#y = tn_pt2(xdata, td=1, k=2, w0=2, d=0.9)
#y = func(xdata, 10, 5, 0, 1)
#print len(y), len(xdata)
#y_noise = 0.1 * np.random.normal(size=xdata.size)
#ydata = y + y_noise
#ydata = tn_pt2(xdata, 0, 2, 1, 5)
#plt.plot(xdata, y, 'g-', label='data')
#plt.plot(xdata, ydata, 'b-', label='data_noise')
#tn_pt2(zip([0, 1],[0, 0.3]), td=0, k=2, w0=1, d=1) ##TODO CORRECT !!= 0 -> https://de.wikipedia.org/wiki/Datei:Step-PT2.svg
#plt.plot(xdata, tn_pt2(xdata, td=0, k=2, w0=1, d=0.2), 'b-', label='data')
#plt.plot(xdata, tn_pt2(xdata, td=0, k=2, w0=1, d=1), 'g-', label='data')
#plt.plot(xdata, tn_pt2(xdata, td=0, k=2, w0=1, d=5), 'r-', label='data')
#tn_pt2(xdata, td=0, k=2, w0=1, d=1)
In [ ]:
#http://www.eit.hs-karlsruhe.de/mesysto/teil-a-zeitkontinuierliche-signale-und-systeme/uebertragungsglieder-der-regelungstechnik/zusammengesetzte-uebertragungsglieder/pt2-glied.html
#xt is zip (xdata, tdata)
def pt2(xt, K, d, T, delay=0):
y = np.array([])
for x, tx in xt:
#For now no delay:
if tx < delay:
y = np.append(y, [0.0])
else:
t = tx - delay
if d > 1: #aperiodischer Fall
T1, T2 = (T/(d+np.sqrt(d**2-1)), T/(d-np.sqrt(d**2-1)))
#print 'T1, T2: ', T1, T2
h = K * (1.0 - 1.0/(T1-T2) * (T1*np.exp(-t/T1)-T2*np.exp(-t/T2)))
y = np.append(y, [h*x])
elif d == 1: #aperiodischer Grenzfall
h = K * (1.0 - (1.0 + t/T)*np.exp(-t/T))
y = np.append(y, [h*x])
else: #periodischer Fall d<1
h = K * (1.0 + 1.0/(np.sqrt(1-d**2)) * np.exp(-d*t/T) * np.cos(np.sqrt(1-d**2)/T * t - np.pi - np.arctan(-d/np.sqrt(1-d**2))))
y = np.append(y, [h*x])
return y
In [ ]:
t = np.arange(0, 10, 1)
x = np.ones(len(t))
pt2(xt=zip(x, t), K=2, d=5, T=1)
In [ ]:
t = np.arange(0, 70, 0.1)
x = np.ones(len(t))
xdata = zip(t, x)
#y = tn_pt2(xdata, td=1, k=2, w0=2, d=0.9)
#y = func(xdata, 10, 5, 0, 1)
#print len(y), len(xdata)
#y_noise = 0.1 * np.random.normal(size=xdata.size)
#ydata = y + y_noise
#ydata = tn_pt2(xdata, 0, 2, 1, 5)
plt.plot(t, x, 'g:', label='data')
plt.plot(t, pt2(xt=zip(x, t), K=1, d=2, T=1, delay=10), 'b-', label='d=2')
plt.plot(t, pt2(xt=zip(x, t), K=1, d=1.0, T=1, delay=10), 'g-', label='d=1')
plt.plot(t, pt2(xt=zip(x, t), K=1, d=3, T=1, delay=10), 'r-', label='d=3')
plt.plot(t, pt2(xt=zip(x, t), K=2, d=0.25, T=1), 'b-', label='d=0.25')
plt.plot(t, pt2(xt=zip(x, t), K=2, d=1, T=1, delay=20), 'g-', label='d=1')
plt.plot(t, pt2(xt=zip(x, t), K=2, d=0.5, T=1), 'r-', label='d=0.5')
plt.plot(t, pt2(xt=zip(x, t), K=0.2, d=0, T=1, delay=10), 'r:', label='d=0.2')
#plt.plot(xdata, ydata, 'b-', label='data_noise')
In [ ]:
#http://www.eit.hs-karlsruhe.de/mesysto/teil-a-zeitkontinuierliche-signale-und-systeme/uebertragungsglieder-der-regelungstechnik/zusammengesetzte-uebertragungsglieder/pt2-glied.html
#xdata is normalized for x per delta-time-step
def pt2_func(soll, K, d, T, delay=0):
y = np.array([])
xt = zip(soll, np.arange(0, len(soll), 1))
for x, tx in xt:
#For now no delay:
if tx < delay:
y = np.append(y, [0.0]) # This can actually be not zero but the last value
else:
t = tx - delay
if d > 1: #aperiodischer Fall
T1, T2 = (T/(d+np.sqrt(d**2-1)), T/(d-np.sqrt(d**2-1)))
#print 'T1, T2: ', T1, T2
h = K * (1.0 - 1.0/(T1-T2) * (T1*np.exp(-t/T1)-T2*np.exp(-t/T2)))
y = np.append(y, [h*x])
elif d == 1: #aperiodischer Grenzfall
h = K * (1.0 - (1.0 + t/T)*np.exp(-t/T))
y = np.append(y, [h*x])
else: #periodischer Fall d<1
h = K * (1.0 + 1.0/(np.sqrt(1-d**2)) * np.exp(-d*t/T) * np.cos(np.sqrt(1-d**2)/T * t - np.pi - np.arctan(-d/np.sqrt(1-d**2))))
y = np.append(y, [h*x])
return y
In [ ]:
def fit(soll, ist, maxfev=1000):
#popt, pcov = curve_fit(pt2_func, xdata, ydata, method='dogbox')
try:
popt_damp, pcov_damp = curve_fit(pt2_func, soll, ist,
method='dogbox',
bounds=([-10, 0, 1.0, 0], [100, 5, 10, 80]))
#plt.plot(np.arange(0, len(xdata), 1), pt2_func(xdata, *popt), 'g-', label='fit')
print 'd>1:', np.sum(np.sqrt(np.diag(pcov_damp))), popt_damp
except RuntimeError as e:
print e
popt_damp = None
pcov_damp = None
try:
popt_swing, pcov_swing = curve_fit(pt2_func, soll, ist,
method='dogbox',
bounds=([-10, 0, 0., 0], [100., 1.001, 10, 80]))
#plt.plot(np.arange(0, len(xdata), 1), pt2_func(xdata, *popt), 'r-', label='fit')
print 'd<1:', np.sum(np.sqrt(np.diag(pcov_swing))), popt_swing
except RuntimeError as e:
print e
popt_swing = None
pcov_swing = None
return popt_damp, pcov_damp, popt_swing, pcov_swing
In [ ]:
test_soll = np.ones(1000)
test_ist = pt2_func(test_soll, K=20, d=0.8, T=10, delay=10)
test_noise = 0.01 * np.random.normal(size=test_ist.size)
test_ist_noise = test_ist + test_noise
popt, pcov, popt2, pcov2 = fit(test_soll, test_ist_noise)
plt.plot(np.arange(0, len(test_soll), 1), pt2_func(test_soll, *popt), 'b-', label='d>1')
plt.plot(np.arange(0, len(test_soll), 1), pt2_func(test_soll, *popt2), 'g-', label='d<1')
plt.plot(np.arange(0, len(test_soll), 1), test_ist, 'r:', label='ist')
plt.plot(np.arange(0, len(test_soll), 1), test_soll, 'y-', label='soll')
plt.legend()
In [ ]:
In [ ]:
import pandas as pd
import numpy as np
from pandas import Series, DataFrame, Panel
#%pylab inline
#plt.rcParams['figure.figsize'] = (15, 15)
### get soll ###
df_soll = pd.read_csv('~/catkin_ws/src/pitasc/applications/sysident/step_log/2017-05-18/2017-05-18_11-45-37_control_output.log',
header=0,
names=['time', 'x_soll'])
#remove trailing [
#df_soll['x_soll'] = df_soll['x_soll'].str[2:].astype(float)
df_soll = df_soll.set_index('time')
### get ist ###
df_ist = pd.read_csv('~/catkin_ws/src/pitasc/applications/sysident/step_log/2017-05-18/2017-05-18_11-45-37_task_vel.log',
header=0,
names=['time', 'x_ist'])
#remove trailing [
#df_ist['x_ist'] = df_ist['x_ist'].str[2:].astype(float)
df_ist = df_ist.set_index('time')
### make one df with ist and soll; indexed by time
# Concates both series to one and fills (unknown) data with last valid one
df_ist_soll = pd.concat([df_soll.x_soll, df_ist.x_ist], axis=1).fillna(method='pad')
# Fills first value with 0 (there is no valid before that one)
df_ist_soll = df_ist_soll.fillna(0)
df_ist_soll.plot(ylim=[-0.05, 0.21], style='.-', drawstyle="steps")
In [ ]:
# Make Timeseries and resample
ts_ist_soll = df_ist_soll.set_index(pd.to_datetime(df_ist_soll.index, unit='s')) # TODO make start date usefull
ts_ist_soll_4ms = ts_ist_soll.resample('4ms').pad().fillna(0).astype(float)
ts_ist_soll_4ms.plot(ylim=[-0.05, 0.21], style='.-', drawstyle="steps")
In [ ]:
## Collect all results
frames = []
In [ ]:
ts_ist_soll_4ms.plot(ylim=[-0.05, 0.21], style='.-', drawstyle="steps")
# curvefit
ts_soll = ts_ist_soll_4ms.x_soll.as_matrix()
ts_ist = ts_ist_soll_4ms.x_ist.as_matrix()
popt_damp, pcov_damp, popt_swing, pcov_swing = fit(ts_soll, ts_ist)
t = np.arange(0, len(ts_ist)*0.004, 0.004)
if popt_damp is not None:
ist_est_damp = pt2_func(ts_soll, *popt_damp)
ts_ist_curvefit1 = pd.Series(ist_est_damp, index=ts_ist_soll_4ms.index, name='x_ist_curvefit_damp')
ts_ist_curvefit1.plot()
frames.append(ts_ist_curvefit1.to_frame())
if popt_swing is not None:
ist_est_swing = pt2_func(ts_soll, *popt_swing)
ts_ist_curvefit2 = pd.Series(ist_est_swing, index=ts_ist_soll_4ms.index, name='x_ist_curvefit_swing')
ts_ist_curvefit2.plot()
frames.append(ts_ist_curvefit2.to_frame())
#plt.plot(t, ist_est_damp, 'b-', label='d>1')
#plt.plot(t, ist_est_swing, 'g-', label='d<1')
#plt.legend()
In [ ]:
In [ ]:
#polyfit
z = np.polyfit(t, ts_ist, 5)
p = np.poly1d(z)
#print 'resampled p'
#print p
ts_ist_soll_4ms.plot(ylim=[-0.05, 0.21], style='.-', drawstyle="steps")
ts_ist_polyfit1 = pd.Series(p(t), index=ts_ist_soll_4ms.index, name='x_ist_ployfit_5')
ts_ist_polyfit1.plot()
frames.append(ts_ist_polyfit1.to_frame())
In [ ]:
#polyfit on nonresampled data
z2 = np.polyfit(df_ist_soll.index, df_ist_soll.x_ist.as_matrix(), 5)
p2 = np.poly1d(z2)
#print 'nonresampled p'
#print p
ts_ist_polyfit2 = pd.Series(p2(t), index=ts_ist_soll_4ms.index, name='x_ist_ployfit2_5')
#ts_ist_polyfit2= ts_ist_polyfit2.resample('4ms').pad().fillna(0)
#s3 = pd.concat([s_4ms, pl2], axis=1).fillna(method='pad').fillna(0)
#y_data_raw.time.as_matrix()
#plt.figure()
ts_ist_soll_4ms.plot(ylim=[-0.05, 0.21], style='.-', drawstyle="steps")
ts_ist_polyfit2.plot()
frames.append(ts_ist_polyfit2.to_frame())
In [ ]:
## Merge all series together to one showable dataframe
frames.append(ts_ist_soll_4ms)
all_results = pd.concat(frames, axis=1)
In [ ]:
all_results.plot(ylim=[-0.05, 0.21], style='', drawstyle="steps")
In [ ]: