Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/STA24_Interpolate_TEST02.py Secret

Created May 21, 2024 22:30
Show Gist options
  • Save dj1711572002/7c6da6feb5ff63ef742e33518c157899 to your computer and use it in GitHub Desktop.
Save dj1711572002/7c6da6feb5ff63ef742e33518c157899 to your computer and use it in GitHub Desktop.
Download ZIP
STA24 Python 2nd Spline Interpolate Plot
Raw
STA24_Interpolate_TEST02.py
import numpy as np
import matplotlib.pyplot as plt
from scipy import interpolate
import pandas as pd
import tkinter
import tkinter.filedialog as FileDialog
import os
import sys
def peaks(df):
df["headMotpeaks_B"]=0
lendf=len(df)
n=0
diffn=[]
for j in range(1,lendf-1):
if df.loc[j,"diffzero"]>0:#diffzeroの番号
n=int(df.loc[j,"diffzero"])
diffn.append(j)
return diffn
#最も近い値検索関数
def idx_of_the_nearest(data, value):
print("data=",data)
print("value=",value)
idx = np.argmin(np.abs(np.array(data) - value))
return idx
def main():
ip1 = ["Nearest", lambda x, y: interpolate.interp1d(x, y, kind="nearest")]#最近傍点補間"
ip2 = ["LInear", interpolate.interp1d]#"線形補間"
ip3 = ["Lagrange", interpolate.lagrange]#"ラグランジュ補間"
ip4 = ["BaryCenter", interpolate.BarycentricInterpolator]#"重心補間"
ip5 = ["Krogh", interpolate.KroghInterpolator]#"Krogh補間"
ip6 = ["2dSpline", lambda x, y: interpolate.interp1d(x, y, kind="quadratic")]#"2次スプライン補間"
ip7 = ["3dSpline", lambda x, y: interpolate.interp1d(x, y, kind="cubic")]#"3次スプライン補間"
ip8 = ["Akima", interpolate.Akima1DInterpolator]#"秋間補間"
ip9 = ["Pch", interpolate.PchipInterpolator]#"区分的 3 次エルミート補間"
kaisu=0
BRBNflag=False
list_interP=[]
list_interPhM=[]
list_interP_hD=[]
list_interPhD=[]
diffN=[]
while(True):
#===READ FILE
print("kaisu=",kaisu)
print("<<<<PUSH KEY::r:read BRBNT s:readBNO ,p:plot e=EXIT>>>> ")
v= input(" keyin =>ENTER")
if v=='r':#-----------------------------------------------------------------------------
root = tkinter.Tk()
root.withdraw()
iDir = r'C:/RTK_Log/'
fTyp = [("データファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
# filenameからbasenameをとりだしてファイル種類区別
basename = os.path.basename(filename)
BRBNflag = "BRBN" in basename
#df
df_BRBNT = pd.read_csv(filename, low_memory=True)
cols=df_BRBNT.columns
#print(cols)
df_BRBNT[cols]=df_BRBNT[cols].astype("float64")
#iTOW_B の末尾ゼロ削除
df_BRBNT = df_BRBNT[df_BRBNT['iTOW_B'] != 0]
#iTOW_Bからゼロ開始のiTOW_B0列作成
df_BRBNT["iTOW_B0"]=df_BRBNT["iTOW_B"]-df_BRBNT.loc[0,"iTOW_B"]
print(df_BRBNT["iTOW_B0"])
df_BRBNT.to_csv(r'C:/RTK_Log/BRBNTi_' + basename)#BRBNTi_補間ファイル保存で完成
dflen=len(df_BRBNT)
#print(dflen,":[]",df_BRBNT.loc[0,"iTOW_B"],"-",df_BRBNT.loc[dflen-1,"iTOW_B"],"]")
#headMotのピーク値の範囲を検索
diffN=peaks(df_BRBNT)
print(diffN)
n0=diffN[kaisu]
n1=diffN[kaisu+3]
hMRange=df_BRBNT.loc[n0:n1,"headMotMA3"]
iTOWRange=df_BRBNT.loc[n0:n1,"iTOW_B0"]
y_observed=hMRange.to_numpy()
x_observed=iTOWRange.to_numpy()
print("x_observed",x_observed)
print("y_observed",y_observed)
# full data range plot
ns=0
ne=len(df_BRBNT)
hMfull=df_BRBNT.loc[ns:None,"headMotMA3"]
iTOWfull=df_BRBNT.loc[ns:ne,"iTOW_B0"]
y_full=hMfull.to_numpy()
x_full=iTOWfull.to_numpy()
plt.scatter(x_full, y_full)
plt.grid()
plt.show()
#指定範囲プロット
#plt.scatter(x_observed, y_observed)
#plt.grid()
#plt.show()
# read file & rtkdata input end----------------------------------------------------------
elif v=='o':#----------------------------------------------------------------------------
f = lambda x: 1/(1 + np.exp(-x))
g = lambda x: 1.0/(1.0+x**2)
h = lambda x: np.sin(x)
x_observed = np.linspace(-10, 10, 11)
#===pandas dfから補間するデータseriesをx_observe y_observedへ代入================
#x_observed numpy ndarrayで計算に渡す　
#pandas seriesからndaary に変換　　s = df['A']　　s.to_numpy()
# Orginal data input end------------------------------------------------------------------
elif v=='s' :# interPolate　補間 計算　SAVE CSV============================================================================================
print("<<<<BRBNT READ OK>>>>")
iTOW_B_init=df_BRBNT.loc[0,"iTOW_B"]
while kaisu<len(diffN)-1:
n0=diffN[kaisu]
n1=diffN[kaisu+3]
hMRange=df_BRBNT.loc[n0:n1,"headMotMA3"]
hDRange=df_BRBNT.loc[n0:n1,"Heading360"]
iTOWB0Range=df_BRBNT.loc[n0:n1,"iTOW_B0"]
iTOWB_start=df_BRBNT.loc[n0,"iTOW_B"]
y_observed=hMRange.to_numpy()
y_observed_hD=hDRange.to_numpy()
x_observed=iTOWB0Range.to_numpy()
print(kaisu,n0,diffN[kaisu],iTOW_B_init,iTOWB_start,iTOWB0Range[n0])
lastn=len(x_observed)-1
kizami=int((x_observed[lastn]-x_observed[0])/10)#10msec刻み
x_latent =np.linspace(x_observed[0], x_observed[lastn], kizami)
iTOWB_kizami=np.linspace(x_observed[0]+iTOW_B_init, x_observed[lastn]+iTOW_B_init, kizami)
#print("iTOWB_start=",iTOWB_start,"iTOWB_kizami=",iTOWB_kizami[0],iTOWB_kizami[1])
#for method_name, method in [ip1, ip2, ip3, ip4, ip5, ip6, ip7, ip8, ip9]:
for method_name, method in [ ip6]:
print(method_name)
fig_idx = 0
txt=":"+method_name+" Range="+str(n0)+"-"+str(n1)
fitted_curve = method(x_observed, y_observed)
fitted_curve_hD=method(x_observed, y_observed_hD)
fit_result=fitted_curve(x_latent)
fit_result_hD=fitted_curve_hD(x_latent)
#print("fit_result=",fit_result)
#dfに変換してログ
list_iTOW_B=iTOWB_kizami.tolist()
list_interP = fit_result.tolist()
list_interP_hD=fit_result_hD.tolist()
list_interPhM.append(list_iTOW_B)
list_interPhM.append(list_interP)
list_interPhD.append(list_iTOW_B)
list_interPhD.append(list_interP_hD)
print("kaisu=",kaisu,":list_interPhM len=",len(list_interPhM),len(list_interP))
print("kaisu=",kaisu,":list_interPhD len=",len(list_interPhD),len(list_interP_hD))
kaisu+=3
#while ------------------------------------------------------------------------------------
df_interPhM = pd.DataFrame(list_interPhM)
df_interPhM=df_interPhM.transpose()
df_interPhD = pd.DataFrame(list_interPhD)
df_interPhD=df_interPhD.transpose()
#df_CPLT = df[["headMotMA3_T", "yawH_T","rpEB_BT","rpNB_BT","rpER_BT","rpNR_BT","relPosE_T","relPosN_T"]]
#print("csv add4")
df_interPhM.to_csv(r'C:/RTK_Log/interPhM_' + basename, index=False) # CSV save
df_interPhD.to_csv(r'C:/RTK_Log/interPhD_' + basename, index=False) # CSV sav
# if"s" End====================================================================================================
elif v=='b' :#RTK補間データをBNOと同期合体　BNOファイルを読み込んでdf_interPhDと合体
print(">>>> SELECT BNO FILE")
root = tkinter.Tk()
root.withdraw()
iDir = r'C:/RTK_Log/'
fTyp = [("BNOデータファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
# filenameからbasenameをとりだしてファイル種類区別
basenameBNO = os.path.basename(filename)
BRBNflag = "BRBN" in basename
#df
df_BNOfull = pd.read_csv(filename, low_memory=True)
print("df_BNOfull:",df_BNOfull.shape,basenameBNO)
print(df_BNOfull)
print(">>>>SELECT interPhD FILE")
fTyp = [("interPhDデータファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
basenamePhD = os.path.basename(filename)
BRBNflag = "BRBN" in basename
#df
df_interPhD = pd.read_csv(filename, low_memory=True)
print("df_interPhD:",df_interPhD.shape,basenamePhD)
#BNITOWの整頓
iTOW_B_start=int(df_BRBNT.loc[diffN[0],"iTOW_B"])
print(df_interPhD)
data=[]
data=df_BNOfull.iloc[:,0]
value=iTOW_B_start
aidx=idx_of_the_nearest(data, value)#BNO　頭出し
print(aidx,df_BNOfull.iloc[aidx,0],value)
df_BNOhalf=df_BNOfull.iloc[aidx:,:]
print("df_BNOhalf=",df_BNOhalf)
n=0# diffNのカウント
# diffN順でinterPhDとBNO
df_ITPBNO=pd.DataFrame()
#df_interPhDを１セットずつとりだしてBNOと合体してCSV　SAVE
st=0
data=[]
coln=len(df_interPhD.columns)
for i in range(0,coln,2):
df_ITPset= pd.DataFrame()
df_BNOset= pd.DataFrame()
df_ITPBNO =pd.DataFrame()
st=i
df_ITPset=df_interPhD.iloc[:,[st,st+1]]
print("==========st=",st,"df_interPhD.iloc[0,st:st+1]=",df_interPhD.iloc[0,st],df_interPhD.iloc[0,st+1],df_ITPset)
#iTOW_B_start=int(df_BRBNT.loc[diffN[i],"iTOW_B"])
ITP_start=df_interPhD.iloc[0,st]
#print(df_interPhD)
#BNO頭出し
data=df_BNOfull.iloc[:,0]#BNITOW列
value=ITP_start#BRBNT iTOWB頭だしターゲット
aidx=idx_of_the_nearest(data, value)#BNO　頭出し
print(">>>>>>>i=",i,"iTOW_B_start=",value,"aidx=",aidx,"df_BNOfull.iloc[aidx,0]=",df_BNOfull.iloc[aidx,0])
num=len(df_ITPset)
df_BNOset=df_BNOfull.iloc[aidx:aidx+num,:]
df_BNOset=df_BNOset.reset_index()
ITPitow=df_ITPset.iloc[:,0]
ilen=len(ITPitow)
print("df_ITPset=",df_ITPset)
print("df_BNOset=",df_BNOset)
df_ITPBNO = pd.concat([df_ITPset, df_BNOset], axis=1)
sa = df_ITPset.iloc[0:ilen,1].sub(df_BNOset.iloc[0:ilen,2])
print(sa)
df_ITPBNO["hD-yaw"]=sa
df_ITPBNO.to_csv(r'C:/RTK_Log/hokan/ITPBNO_' +str(i)+"_" +basename, index=False) # CSV sav
#***************PLOT*******************************************************************************
fig, ax1 = plt.subplots()
fig.suptitle("RTK Heading-BNO yaw No."+str(i))
ax1.set_title("blue:Heading,greem:yaw,red:SUB(Heading-yaw)")
hDRange=df_ITPBNO.iloc[0:ilen,1]
iTOWRange=df_ITPBNO.iloc[0:ilen,0]
saRange=df_ITPBNO.loc[0:ilen,"hD-yaw"]
y_hD=hDRange.to_numpy()
x_itow=iTOWRange.to_numpy()
yawRange=df_ITPBNO.iloc[0:ilen,4]
y_yaw=yawRange.to_numpy()
y_sa=saRange.to_numpy()
print("x_itow=",x_itow)
print("y_sa=",y_sa)
# y1軸の散布図を描画
ax1.scatter(x_itow, y_hD, color='blue', alpha=0.5, label='deg')
ax1.scatter(x_itow, y_yaw, color='green', alpha=0.5, label='deg')
# y2軸の散布図を描画
ax2 = ax1.twinx()
ax2.scatter(x_itow, y_sa, color='red', alpha=0.5, label='deg')
fig.savefig("C:/RTK_Log/hokan/Grph-"+str(i)+".png")
# plt.scatter(x_itow, y_hD, c="red", label="Heading")
#plt.scatter(x_itow, y_yaw, c="blue", label="BNOyaw")
# plt.grid()
plt.show()
main()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment