Obs = np.loadtxt(
'/home/summit/Spidoo_ws/src/stiffness_observer/scripts/PLOT/GOOD/Obse_Stif_V8.txt'
)
WithOb = np.loadtxt(
'/home/summit/Spidoo_ws/src/stiffness_observer/scripts/PLOT/GOOD/Without_Stif_V8.txt'
)
Obs2 = np.loadtxt(
'/home/summit/Spidoo_ws/src/stiffness_observer/scripts/PLOT/GOOD/Obse_Stif_V5.txt'
)
WithOb2 = np.loadtxt(
'/home/summit/Spidoo_ws/src/stiffness_observer/scripts/PLOT/GOOD/Without_Stif_V5.txt'
)
X4 = butter_lowpass_filter(Obs[:, 1], 0.04, 10, 8)
Y4 = butter_lowpass_filter(Obs[:, 2], 0.04, 10, 8)
X2 = butter_lowpass_filter(WithOb[:, 1], 0.04, 10, 8)
Y2 = butter_lowpass_filter(WithOb[:, 2], 0.04, 10, 8)
Xref = butter_lowpass_filter(Obs[:, 10], 0.04, 10, 8)
Yref = butter_lowpass_filter(Obs[:, 11], 0.04, 10, 8)
Xref = Xref[:] + X4[0] - Xref[0]
Yref = Yref[:] + Y4[0] - Yref[0]
#plt.plot(res1[:,10],res1[:,11], 'b', linewidth=4.0)
plt.plot(Xref, Yref, 'g', linewidth=4.5)
plt.plot(X4, Y4, 'r--', linewidth=4)
plt.plot(X2, Y2, 'b--', linewidth=4)
while (i<len(Obs2[:,1])-1):
s44[i+1]=s44[i]+hypot(Obs2[i+1,1]-Obs2[i,1],Obs2[i+1,2]-Obs2[i,2])
i=i+1
i=0
while (i<len(WithOb2[:,1])-1):
s22[i+1]=s22[i]+hypot(WithOb2[i+1,1]-WithOb2[i,1],WithOb2[i+1,2]-WithOb2[i,2])
i=i+1
#############All errors ############################################################################
##########################################################################
y4= butter_lowpass_filter(Obs[:,4], 0.03, 200, 6)
y2= butter_lowpass_filter(WithOb[:,4], 0.03, 200, 6)
y44= butter_lowpass_filter(Obs2[:,4], 0.03, 200, 6)
y22= butter_lowpass_filter(WithOb2[:,4], 0.03, 200, 6)
plt.plot(s44[:],y44[:], 'r', linewidth=3)
plt.plot(s22[:],y22[:], 'k--', linewidth=3)
plt.plot(s4[:],y4[:], 'b', linewidth=3)
plt.plot(s2[:],y2[:], 'g--', linewidth=3)
plt.xlabel('Curvilinear abscissa [m]', fontsize=18)
plt.ylabel('Lateral error [$m$]', fontsize=18)
from scipy import fftpack
from pylab import *
import shapely.geometry as geom
from scipy import spatial
from ButterFilter import butter_lowpass_filter
kk = 180 / pi
######Trajectory plot #################################################################
res4 = np.loadtxt(
'/home/summit/Spidoo_ws/src/spido_obskridssa/scripts/Plot/Without_contr_4_Rear.txt'
)
X4 = butter_lowpass_filter(res4[:, 1], 0.04, 10, 8)
Y4 = butter_lowpass_filter(res4[:, 2], 0.04, 10, 8)
Xref = butter_lowpass_filter(res4[:, 8], 0.04, 10, 8)
Yref = butter_lowpass_filter(res4[:, 9], 0.04, 10, 8)
Xref = Xref[:] + X4[0] - Xref[0]
Yref = Yref[:] + Y4[0] - Yref[0]
#plt.plot(res1[:,10],res1[:,11], 'b', linewidth=4.0)
plt.plot(X4, Y4, 'r--', linewidth=3.0)
plt.plot(Xref, Yref, 'g', linewidth=3.0)
plt.axis('equal')
#plt.axis([-50, 10, -10, 70])
plt.yticks(color='k', size=40)
from scipy import fftpack
from pylab import *
import shapely.geometry as geom
from scipy import spatial
from ButterFilter import butter_lowpass_filter
kk = 180 / pi
######Trajectory plot #################################################################
res4 = np.loadtxt(
'/home/summit/Spidoo_ws/src/stiffness_observer/scripts/PLOT/LQR_Stif_V5.txt'
)
X4 = butter_lowpass_filter(res4[:, 1], 0.04, 10, 8)
Y4 = butter_lowpass_filter(res4[:, 2], 0.04, 10, 8)
Xref = butter_lowpass_filter(res4[:, 10], 0.04, 10, 8)
Yref = butter_lowpass_filter(res4[:, 11], 0.04, 10, 8)
Xref = Xref[:] + X4[0] - Xref[0]
Yref = Yref[:] + Y4[0] - Yref[0]
#plt.plot(res1[:,10],res1[:,11], 'b', linewidth=4.0)
plt.plot(X4, Y4, 'r--', linewidth=3.0)
plt.plot(Xref, Yref, 'g', linewidth=3.0)
plt.axis('equal')
#plt.axis([-50, 10, -10, 70])
plt.yticks(color='k', size=40)
s2[i + 1] = s2[i] + hypot(WithOb[i + 1, 1] - WithOb[i, 1],
WithOb[i + 1, 2] - WithOb[i, 2])
i = i + 1
s44 = np.zeros(len(sim[:, 1]))
i = 0
while (i < len(sim[:, 1]) - 1):
s44[i + 1] = s44[i] + hypot(sim[i + 1, 1] - sim[i, 1],
sim[i + 1, 2] - sim[i, 2])
i = i + 1
print(len(s4))
##################
yf4 = butter_lowpass_filter(sim[:, 20], 0.02, 200, 6)
yf5 = butter_lowpass_filter(sim[:, 21], 0.02, 200, 6)
yf2 = butter_lowpass_filter(WithOb[:, 20], 0.02, 200, 6)
yf3 = butter_lowpass_filter(WithOb[:, 21], 0.02, 200, 6)
X4 = butter_lowpass_filter(FyF, 0.007, 10, 8)
#for i in range(len(X4)/2):
#if X4[i]>0:
#X4[i]=X4[i]+100*sin(10**-1.5*i)
#X4[len(X4)/2+i-1]=-yf4[len(X4)/2+i-1]+80*sin(10**-2*i)
#### V=5m:s
#for i in range(len(X4)):
# if i<980:
q.append(Vpsi_gazebo[i])
print(np.shape(res[:, 3]))
print(len(Vy_gazebo))
print(np.shape(Vx_gazebo))
for i in range(len(Vy_gazebo)):
if i % 2 == 0:
Vy = Vy_gazebo[i] * math.cos(res[
i / 2, 3] - pi) - Vx_gazebo[i] * math.sin(res[i / 2, 3] - pi) - 1.9
k.append(Vy)
#for i in range(len(k)):
# k[i]=-k[i]
X4 = butter_lowpass_filter(k[:], 0.8, 10, 8)
q4 = butter_lowpass_filter(q[:], 0.04, 10, 8)
yf4 = butter_lowpass_filter(sim[:, 6], 0.02, 200, 6)
############################ Curvilinear abscissa ##############################
s4 = np.zeros(len(res[:, 1]))
i = 0
while (i < len(res[:, 1]) - 1):
s4[i +
1] = s4[i] + hypot(res[i + 1, 1] - res[i, 1], res[i + 1, 2] - res[i, 2])
i = i + 1
s44 = np.zeros(len(sim[:, 1]))
from ButterFilter import butter_lowpass_filter
kk = 180 / pi
###### Upload recorded files #################################################################
NLobs = np.loadtxt(
'/home/summit/Spidoo_ws/src/spido_obskridssa/scripts/Plot/NLObser_4.txt'
) #NLobser_5
LinObs = np.loadtxt(
'/home/summit/Spidoo_ws/src/spido_obskridssa/scripts/Plot/LinObser_4.txt')
WithoutObs = np.loadtxt(
'/home/summit/Spidoo_ws/src/spido_obskridssa/scripts/Plot/Without_contr_4.txt'
)
XNL = butter_lowpass_filter(NLobs[:, 1], 0.04, 10, 8)
YNL = butter_lowpass_filter(NLobs[:, 2], 0.04, 10, 8)
Xlin = butter_lowpass_filter(LinObs[:, 1], 0.04, 10, 8)
Ylin = butter_lowpass_filter(LinObs[:, 2], 0.04, 10, 8)
Xwo = butter_lowpass_filter(WithoutObs[:, 1], 0.04, 10, 8)
Ywo = butter_lowpass_filter(WithoutObs[:, 2], 0.04, 10, 8)
Xref = butter_lowpass_filter(NLobs[:, 8], 0.04, 10, 8)
Yref = butter_lowpass_filter(NLobs[:, 9], 0.04, 10, 8)
Xref = Xref[:] + XNL[0] - Xref[0]
Yref = Yref[:] + YNL[0] - Yref[0]
###### Trajectory plot #################################################################
