Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/LEFT_STA24_PPlot_sITPBNO_RelAnalysys_06cl.py Secret

Created June 13, 2024 12:44
Show Gist options
  • Save dj1711572002/02f9ee67b53647ebac083c09b2483d07 to your computer and use it in GitHub Desktop.
Save dj1711572002/02f9ee67b53647ebac083c09b2483d07 to your computer and use it in GitHub Desktop.
Download ZIP
LEFT STA24　Angular plot left ski right turn
Raw
LEFT_STA24_PPlot_sITPBNO_RelAnalysys_06cl.py
import numpy as np
import seaborn as sns
import matplotlib.pyplot as plt
from scipy import interpolate
from scipy.signal import find_peaks
from scipy import stats
import pandas as pd
from sklearn import datasets
from sklearn.model_selection import train_test_split
from sklearn.preprocessing import StandardScaler
from sklearn.svm import SVC
from sklearn.svm import SVR
import tkinter
import tkinter.filedialog as FileDialog
import os
import sys
import pickle
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 peaks10(df):#ITPBNOでのscipy find_peaksでピーク検出　　diffN戻値
srange=40#個数分の範囲でピーク探す
prange=100#100度以上差がでる
#手検索
dif=df["headMot360"].diff()
#print("dif=",dif)
diffN=[]
n=0
zeroflag=0
for i in range(2, len(df)):
vrange=abs(df.loc[i,"headMot360"]-df.loc[zeroflag,"headMot360"])
#変曲点と40個間隔と値差6０度以上
if dif[i] * dif[i-1] < 0 and i-zeroflag>40 and vrange>60 :
diffN.append(i)
zeroflag = i
#print("diffN=",diffN)
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 peaks10col(df,colname):#ITPBNOでのscipy find_peaksでピーク検出　　diffN戻値
srange=40#個数分の範囲でピーク探す
prange=100#100度以上差がでる
#手検索
dif=df[colname].diff()
#print("dif=",dif)
diffN=[]
n=0
zeroflag=0
for i in range(2, len(df)):
#print(df.loc[i,colname])
#print(df.loc[zeroflag,colname])
vrange=abs(df.loc[i,colname]-df.loc[zeroflag,colname])
#print("vrange=",vrange)
#変曲点と40個間隔と値差6０度以上
if dif[i] * dif[i-1] < 0 and i-zeroflag>40 and vrange>60 :
diffN.append(i)
zeroflag = i
#print("diffN=",diffN)
return diffN
#==========================================================================
# def onerow(matrix) 　numpy array行列の行を１行にまとめる
def onerow(matrix):
target=[]
rown=matrix.shape[0]
coln=matrix.shape[1]
for i in range(rown):
for j in range(coln):
target.append(str(matrix[i,j]))
return target
#=======================SVM TRAINING DF 生成============================================
#"sITPBNO_54_True_BRBNTver042_03-15-11-39-44.binShorten_BTBNO.csv"から番号54を置換
def sITP_concat(fn,listN):
print(fn)
fn1 = os.path.basename(fn)
_1st=int(fn1.find('_'))
_2nd=int(fn1.find('_',_1st+1))
nostr=fn1[_1st:_2nd+1]
i=0
for istr in listN:
tstr='_'+istr+'_'
newfn=fn1.replace(nostr,tstr)
print(i,":",nostr,"=",newfn)
#df_BNOファイル読み込み---------------------------------------------------------------
if i==0:
df0 = pd.read_csv( r'C:/RTK_Log/hokan/'+newfn, low_memory=True)
print(df0.shape)
else:
df_add = pd.read_csv( r'C:/RTK_Log/hokan/'+newfn, low_memory=True)
df0=pd.concat([df0,df_add],axis=0)
print(df0.shape)
i+=1
return df0
def sITP_dfMake(fn,listN,colist,flag): #sITPファイル群を指定番号で1本のdfに合体　新規列追加
print(fn)
print(listN)
print(colist)
print(flag)
fn1 = os.path.basename(fn)
_1st=int(fn1.find('_'))
_2nd=int(fn1.find('_',_1st+1))
nostr=fn1[_1st:_2nd+1]
i=0
for istr in listN:
tstr='_'+istr+'_'
newfn=fn1.replace(nostr,tstr)
print(i,":",nostr,"=",newfn)
#df_BNOファイル読み込み---------------------------------------------------------------
if i==0:
df0 = pd.read_csv( r'C:/RTK_Log/hokan/'+newfn, low_memory=True)
print(df0.shape)
else:
df_add = pd.read_csv( r'C:/RTK_Log/hokan/'+newfn, low_memory=True)
df0=pd.concat([df0,df_add],axis=0)
print(df0.shape)
i+=1
#===========df0　インデクスリセット==========
df0=df0.reset_index()
#===========headMotdiff追加ターン方向パラメータ
df0["headMotdiff"]=df0["headMot360"].diff()
df0.loc[0,"headMotdiff"]=0
#============Skid追加====================
df0["skid"]=df0["headMot360"]-df0["Heading360"]
print("skid=",df0["skid"])
#============Skidyaw追加==================
df0["skidyaw"]=df0["headMot360"]-df0["yaw"]
print("skidyaw=",df0["skidyaw"])
#==========カラム選択=====================================================
df1=df0[colist]
print("df1.columns=",df1.columns)
print("df1.shape=",df1.shape)
#=============振幅追加====================
Shinpuku(df1,fn,listN)
#Standard Scalerで正規化--------------------------------------------------
if flag=="train":
from sklearn.preprocessing import StandardScaler
std_scaler = StandardScaler()
#df Scaling
std_scaler.fit(df1)
df1_std = pd.DataFrame(std_scaler.transform(df1), columns=df1.columns)
#print("df_std.columns=",df1_std.columns)
#print("df_std.shape=",df1_std.shape)
elif flag=="target":
df1_std=df1
elif flag=="org" :#読み込んで合体したそのままのdf0を返す
df1_std=df0
return df1_std
def Shinpuku(df,fn,listN):#colnameの振幅列追加
pd.set_option('display.max_rows', None)
#print("振幅列作成　diffNを使って振幅計算")
print(df.columns)
print("df=",df)
peaks_headMot=peaks10col(df,"headMot360")
idx_max_headMot=df["headMot360"].idxmax()
idx_min_headMot=df["headMot360"].idxmin()
max_headMot=df["headMot360"].max()
min_headMot=df["headMot360"].min()
print("idx_max_headMot[",idx_max_headMot,"]=",max_headMot)
print("idx_min_headMot[",idx_min_headMot,"]=",min_headMot)
print("peaks_headMot_index=",peaks_headMot)
print("peaks_headMot=",df.loc[peaks_headMot,"headMot360"])
peaks_Heading=peaks10col(df,"Heading360")
idx_max_Heading=df["Heading360"].idxmax()
idx_min_Heading=df["Heading360"].idxmin()
max_Heading=df["Heading360"].max()
min_Heading=df["Heading360"].min()
print("idx_max_Heading[",idx_max_Heading,"]=",max_Heading)
print("idx_min_Heading[]",idx_min_Heading,"]=",min_Heading)
print("peaks_Heading360_index=",peaks_Heading)
print("peaks_Heading360=",df.loc[peaks_Heading,"Heading360"])
peaks_yaw=peaks10col(df,"yaw")
idx_max_yaw=df["yaw"].idxmax()
idx_min_yaw=df["yaw"].idxmin()
max_yaw=df["yaw"].max()
min_yaw=df["yaw"].min()
print("idx_max_yaw[",idx_max_yaw,"]=",max_yaw)
print("idx_min_yaw[",idx_min_yaw,"]=",min_yaw)
print("peaks_yaw_index=",peaks_yaw)
df_yaw56=df.loc[peaks_yaw,"yaw"]+56
print("peaks_yaw65=",df_yaw56)
# plot
fig = plt.figure()
# add_subplot()でグラフを描画する領域を追加する．引数は行，列，場所
ax1 = fig.add_subplot(1,1, 1)
#ax2 = fig.add_subplot(2,1, 2) #ax1.twinx()
#実データプロット
xj0=np.arange(len(df))
#print("x0=",x0)
yj0=df["headMot360"].to_numpy()
yj1=df["Heading360"].to_numpy()
df_yaw=df["yaw"]+56
yj2=df_yaw[:].to_numpy()
yj3=df["roll"].to_numpy()
yj4=(df["headMotdiff"]*10).to_numpy()
yj5=(df["H-Ysa360"]*2).to_numpy()
yj6=df["skid"].to_numpy()
yj7=(df["skidyaw"]-56).to_numpy()
x1=np.array(peaks_headMot)
y1=df.loc[peaks_headMot,"headMot360"].to_numpy()
x11=np.array(peaks_Heading)
y11=df.loc[peaks_Heading,"Heading360"].to_numpy()
x12=np.array(peaks_yaw)
y12=df_yaw56[:].to_numpy()
#max min値
x2=np.array(idx_max_headMot)
y2=np.array(max_headMot)
#y11=lower1.to_numpy()
#y12=upper1.to_numpy()
#y12=list_In1.to_numpy()
ax1.plot(xj0,yj0,color="blue",label="headMot360")
ax1.plot(xj0,yj1,color="red",label="Heading360")
ax1.plot(xj0,yj2,color="green",label="yaw")
ax1.plot(xj0,yj3,color="black",label="roll")
ax1.plot(xj0,yj4,color="aqua",label="headMotdiff")
ax1.plot(xj0,yj5,color="orange",label="H-Ysa360")
ax1.plot(xj0,yj6,color="violet",label="skid")
ax1.plot(xj0,yj7,color="brown",label="skidyaw")
ax1.plot(x1,y1, color="blue",marker = 'o',linewidth=0)
ax1.plot(x11,y11, color="red",marker = 'o', linewidth=0)
ax1.plot(x12,y12,color="green",marker = 'o' ,linewidth=0)
#ax1.plot(x2,y2,color="orange", label="headMot1_AS")
ax1.grid()
listNstr= ','.join(listN)
title=listNstr+fn
fig.suptitle(title, y=0.995,size=10, weight=2, color="red")
plt.legend(loc="upper left")
plt.show()
print(fn,listNstr)
txt=fn+"TIME_"+listNstr
fig.savefig(txt+"_ppr.png")
#df_targetの実データからランクに分けて新しいdf作成
def tRank_make(df,rankN,values,colname):
#rank=targetのランク値LIST　values=ランクを分ける上下区切り数２次元LIST
print("rankN=",rankN)
print("values=",values)
print("colname=",colname)
#colnameをTraining用ターゲットとしてランク分け--------------------------------
errors=df.loc[:,colname].to_numpy()
errAve=np.average(errors)
errMax=np.max(errors)
errMin=np.min(errors)
print("Average=",errAve,"Max=",errMax,"Min=",errMin)
y_targetVal=errors
#df_target=pd.DataFrame()
#df_target["H-Ysa360"]=df_target["H-Ysa360"]
list_target=[]
for i in range(0,len(df)):
d=y_targetVal[i]-errAve
for ri in range(0,len(rankN)):
#print("values[ri][0]=",ri,values[ri][0])
if (abs(d)>values[ri][0]) and (abs(d)<=values[ri][1]):# [ri,0]-[ri,1]以内ならランク rankN[ri]
list_target.append(rankN[ri])
#print("list_taget.len,list_target=",len(list_target),list_target)
df=pd.DataFrame(list_target)
print("targetmake:df=",df)
return df
#ITPBNO_make START==============================================================================
def ITPAll_make(fn):#ファイル名引数　interPDM ファイル群を合体して１本のDFにする
print("ITPAll make")
#シリーズのファイル名を指定して順番に読み込んで合体していく
#root = tkinter.Tk()
#root.withdraw()
#iDir = r'C:/RTK_Log/hokan'
#fTyp = [("BNOデータファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
#filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
# filenameからbasenameをとりだしてファイル種類区別
basename1 = os.path.basename(fn)
BRBNflag = "BRBN" in basename1
#df_BNOファイル読み込み---------------------------------------------------------------
df = pd.read_csv(fn, low_memory=True)
#print(basename1)
#-----Path内のファイル名を取得して、対象ファイルの数を数える----------------------------
datepos=int(basename1.find('-'))
datestr=basename1[datepos-2:datepos+9]
#print("datestr=",datestr)
dir_path = 'C:/RTK_Log/hokan'
files = os.listdir(dir_path)
#print("files=",files)
#datestrとcsvを含むファイル取得----------------------
st=[]
ITPBNO_in=[]
for i in range(0,len(files)):
st=files[i]
#print("st[0]=",st[0:6])
nsu=len("interPDMRTK")
if st[0:nsu]=="interPDMRTK":
print(i,":",st)
ITPBNO_in.append(st)#ITPBNOが先頭文字のファイル抽出
date_in = [s for s in ITPBNO_in if datestr in s]#datestr日付があっているファイル抽出
notpng_in=[s for s in date_in if'.png' not in s]#pngを除外
csv_in = [s for s in notpng_in if '.csv' in s]#csv抽出
#print("date_in=",date_in)
#print("notpng_in=",notpng_in)
#print("csv_in=",csv_in)
#print("len=",len(csv_in))
list_str=[]
list_val=[]
#ファイルDIFリストから番号LISTを作成する------
for i in range(0,len(csv_in)):
fname=csv_in[i]
_1st=int(fname.find('_'))
_2nd=int(fname.find('_',_1st+1))
nostr=fname[_1st:_2nd+1]
no=int(nostr.replace('_', ''))
print(i,":",nostr,"=",fname)
list_str.append(nostr)
list_val.append(no)
list_sorted=sorted(list_val)
#print("list_str=",list_str)
#print("list_val=",list_val)
#print("list_sorted=",list_sorted)
#print("max=",max(list_val),"min=",min(list_val))
#File名分解-------------------------------------
fname=csv_in[0]
_1st=int(fname.find('_'))
_2nd=int(fname.find('_',_1st+1))
mae_name=fname[:_1st]
ato_name=fname[_2nd+1:]
#print("mae_name=",mae_name,"ato_name=",ato_name)
df_ITPAll=pd.DataFrame()
df_dummy=pd.DataFrame()
df_dess=pd.DataFrame()
#順に読み込みDF作成------------------------------
for i in range(0,len(list_val)-1):#最後のデータセットはノイズなので除去
df_dummy=df_dummy[:0]
fnostr="_"+str(list_sorted[i])+"_"
fn='C:/RTK_Log/hokan/'+mae_name+fnostr+ato_name
#print(i,":",fn)
df_dummy = pd.read_csv(fn,low_memory=True)#interPDMRTKファイル
#print(df_dummy)
df_dummy1=df_dummy.dropna(how='any')
df_dummy1=df_dummy1.iloc[1:,:]
df_ITPAll = pd.concat([df_ITPAll, df_dummy1],axis=0)
df_dummy2=df_dummy1.describe()
df_dess=pd.concat([df_dess,df_dummy2],axis=0)
#print(i,":","df_ITPAll=",df_ITPAll)
df_ITPAll=df_ITPAll.reset_index()
print("df_ITPAll=",df_ITPAll)
df_ITPAll.to_csv(r'C:/RTK_Log/hokan/ITPAll_' +"_" +basename1, index=False) # CSV sav
#df_dess.to_csv(r'C:/RTK_Log/hokan/ITPA_DESS_' +"_" +basename1, index=True) # CSV sav
#describe()統計量ファイルを作成保存
df_des=df_ITPAll.describe()
print("df_des=",df_des)
df_des.to_csv(r'C:/RTK_Log/hokan/ITPAll_DES_' +"_" +basename1, index=True) # CSV sav
return df_ITPAll
print("end")
#ITPBNO_make END=============================================================================
def drop_range(df,colname,max,min):
print("drop_range")
dropindex=[]
for i in range(0,len(df)):
print(i,":df.loc[i,colname]=",df.loc[i,colname],"max=",max,"min=",min)
#print(i+n0,":upper,Value =",upper[i+n0],df_BRBNT.loc[i+n0,colname],ewm_mean[i+n0])
if df.loc[i,colname]>max or df.loc[i,colname]<min:
print(i,">>>>>>>",colname,":value=",df.loc[i,colname])
dropindex.append(i)
return dropindex
#***************************ANGULAR CHECK 角速度上限値で外れ値習性*******************************
def angularCheck(df,angname):
print("angularCheck headMot_B rePosHeading_R headMot360 Heading360 ")
list0_itowB=df["iTOW_B"]
list0_ang=df[angname]
list0_ang_e05=df[angname]*0.00001
list0_ang_diff=list0_ang_e05.diff()
list0_ang_AS=list0_ang_diff/0.12#角速度
ASmax=500
ASmin=-500
dropindex=[]
for i in range(1,len(df)):
if(list0_ang_AS[i]<ASmin or list0_ang_AS[i]>ASmax):
print("DROP:",i,list0_ang_AS[i],ASmax,ASmin)
dropindex.append(i)
#外れ値置き換え
#外れ範囲の前後のデータで直線補間
print("dropindex=",dropindex,len(dropindex))
for i in range(0,len(dropindex)-1):
old_value=df.loc[dropindex[i],angname]
df.loc[dropindex[i],angname]=df.loc[dropindex[i]-1,angname]
print(i,":old value=",old_value,"new value=",df.loc[dropindex[i],angname])
#修理済みデータ
list1_ang_e05=df[angname]*0.00001
list1_ang_diff=list1_ang_e05.diff()
list1_ang_AS=list1_ang_diff/0.12#角速度
#------------------interPの外れ値除去-------------------------
fig = plt.figure()
# add_subplot()でグラフを描画する領域を追加する．引数は行，列，場所
ax1 = fig.add_subplot(2,1, 1)
#ax2 = fig.add_subplot(2,1, 2) #ax1.twinx() # x軸を共有
#ax3 = fig.add_subplot(3,1, 3,title=colname+"_BRBNT1") #ax1.twinx() # x軸を共有
#ax1.set_xlabel('iTOWB_kizami') #x軸ラベル
x1=list0_itowB.to_numpy()
y1=list0_ang_AS.to_numpy()
y11=list1_ang_AS.to_numpy()
y12=list0_ang_e05.to_numpy()
y13=list1_ang_e05.to_numpy()
#y11=lower1.to_numpy()
#y12=upper1.to_numpy()
#y12=list_In1.to_numpy()
ax1.plot(x1,y1, color="blue",marker = 'o', label="headMot_AS")
ax1.plot(x1,y11, color="red", label="headMot1_AS")
ax1.plot(x1,y12,color="green", label="headMot1_AS")
ax1.plot(x1,y13,color="orange", label="headMot1_AS")
#ax1.plot(x1,y12, color="red", label="upper1")
#ax1.plot(x1,y12, color="green", label="ewm_mean1")
#x2=df["iTOW_B"].to_numpy()
#y2=list_relPosH_AS.to_numpy()
#y22=ewm_mean2.to_numpy()
#ax2.plot(x2,y2, color="green", marker = 'o',label="relPosH_AS")
#ax2.plot(x2,y22, color="red", label="ewm_mean2")
#ax3.plot(x_observed,df_BRBNT1.loc[n0:n1,colname], color="red", label=colname+"_BRBNT1")
#plt.scatter(iTOWB_kizami,fit_result)
#plt.scatter(x_observed,y_observed)
#plt.title(colname)
plt.show()
return df
#*****************ITP個別ファイルにBNO列追加******************************************************
def bno_add(df_ITP,df_BNOfull):
data=[]
rown=len(df_ITP)
data=df_BNOfull.iloc[:,0]
iTOW_B_start=df_ITP.loc[0,"iTOW_B"]
lastrow=df_ITP.tail(1)
value=iTOW_B_start
aidx=idx_of_the_nearest(data, value)#BNO　頭出し
lidx=aidx+rown
print(aidx,df_BNOfull.iloc[aidx,0],value)
df_BNOhalf=df_BNOfull.iloc[aidx:lidx,:]
df_BNOhalf=df_BNOhalf.reset_index()
df_ITP=df_ITP.reset_index()
print("df_ITP=",df_ITP)
print("df_BNOfull=",df_BNOfull)
print("df_BNOhalf len=",len(df_BNOhalf),"df_ITP len=",len(df_ITP))
df_new = pd.concat([df_ITP, df_BNOhalf], axis=1)#合体
print(df_new.shape,df_new)
#df_new.to_csv(r'C:/RTK_Log/hokan/df_new.csv', index=False) # CSV sav
#df_ITP.to_csv(r'C:/RTK_Log/hokan/df_ITP.csv', index=False) # CSV sav
#df_BNOhalf.to_csv(r'C:/RTK_Log/hokan/df_BNOhalf.csv', index=False) # CSV sav
return df_new
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=[]
list_interPDM=[]
diffN=[]
while(True):
#===READ FILE
#print("kaisu=",kaisu)
print("<<<<PUSH KEY::r:read BRBNT s:readBNO ,p:plot e=EXIT>>>> ")
v= input(" keyin =>ENTER")
#**************KEY BRANCHES************************************************
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()
#角速度で外れ値チェック
df_new=angularCheck(df_BRBNT,"headMot_B")
df_new=angularCheck(df_new,"Heading360")
df_new.to_csv(r'C:/RTK_Log/AngChecked_dfnew_'+"_"+ basename, index=True) # CSV sav
elif v=='all':
root = tkinter.Tk()
root.withdraw()
iDir = r'C:/RTK_Log/hokan'
print(">>>>SELECT ONE interPDM FILE for All ")
fTyp = [("interPDMデータファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
basenamePDM = os.path.basename(filename)
ITPAll_make(filename)
# 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------------------------------------------------------------------
#**********************************************************************************************************************************************
#*************RTK All Interpolate 全補間　interPDM_ファイル群生成********************************************************************************
elif v=='ho' :#RTK全体の補間と補間後のdiffN計算追加 diffN毎にinterPDMファイル群を生成
print(" df_BRBNT ファイルを読み込み済みのことiTOW処理とdiffNが必要")
iTOW_B_init=df_BRBNT.loc[0,"iTOW_B"]
# lon_B lat_B height_B velN_B velE_B velD_B gSpeed_B headMot_B relPosN_B relPosE_B relPosD_B relPosLength_B relPosHeading_B
# relPosHeading_B lon_R lat_R velN_R velE_R velD_R gSpeed_R headMot_R relPosN_R relPosE_R relPosD_R relPosLength_R relPosHeading_R
# lon_T lat_T height_T velN_T velE_T velD_T gSpeed_T headMot_T relPosN_T relPosE_T relPosD_T relPosLength_T relPosHeading_T
#カラム定義 ２次元LIST
list_call=["iTOW_B","lon_B","lat_B","height_B","velN_B","velE_B","velD_B","gSpeed_B","headMot_B","relPosN_B","relPosE_B","relPosD_B","relPosLength_B","relPosHeading_B"
,"lon_R","lat_R","velN_R","velE_R","velD_R","gSpeed_R","headMot_R","relPosN_R","relPosE_R","relPosD_R","relPosLength_R","relPosHeading_R"
,"lon_T","lat_T","velN_T","velE_T","velD_T","gSpeed_T","headMot_T","relPosN_T","relPosE_T","relPosD_T","headMot360","Heading360","headMotT360","H-Ysa360"]
print("list_call init len =",len(list_call))
#list_call.insert(0, "iTOW_B")
#print("list_call add iTOW_B len =",len(list_call))
#print(list_col[0][0],list_col[0].index("lon_B"),list_col[0][list_col[0].index("lon_B")])
#list_call=list_col[0]+list_col[1]+list_col[2]+"iTOW_B"+"iTOW_B0"#末尾にiTOW_Bのゼロカウント追加
#print("list_call=",list_call)
#<<<<BNO生データファイル読み込み-----------------------------------------------------
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)
basenameBNO = os.path.basename(filename)
df_BNOfull = pd.read_csv(filename, low_memory=True)
df_BNOfull.columns=["bnitow","yaw","pitch","roll","ax","ay","az"]#コラムヘッダ名追加
print("df_BNOfull:",df_BNOfull.shape,basenameBNO)
print(df_BNOfull)
#<<<------------------------------------------------------------------------------
klimit=len(diffN)-3
df_PDMdesAll=pd.DataFrame()
df_BRBNT1=pd.DataFrame()# 外れ値除去df
while kaisu<klimit:#データ区切りdiffN[]で
for i in range(0,len(list_call)):#全パラメータ補間ループ、末尾２個除く
colname=list_call[i]
print(i,":colname=",colname)
n0=diffN[kaisu]
n1=diffN[kaisu+3]
#df_BRBNTの各コラムの外れ値除去　動きのデータの角速度で除外
"""
ewm_span=90
threshold=1.5
ewm_mean = df_BRBNT.loc[n0:n1,colname].ewm(span=ewm_span).mean() # 指数加重移動平均
ewm_std = df_BRBNT.loc[n0:n1,colname].ewm(span=ewm_span).std() # 指数加重移動標準偏差
lower=ewm_mean - ewm_std * threshold
upper=ewm_mean + ewm_std * threshold
print("lower=",lower)
print("n0:n1=",n0,n1,n1-n0)
dropindex=[]
for i in range(0,n1-n0-1):
print(i+n0,":lower,Value =",lower[i+n0],df_BRBNT.loc[i+n0,colname],ewm_mean[i+n0])
print(i+n0,":upper,Value =",upper[i+n0],df_BRBNT.loc[i+n0,colname],ewm_mean[i+n0])
if (df_BRBNT.loc[i+n0,colname]>upper[i+n0]) or (df_BRBNT.loc[i+n0,colname]<lower[i+n0]):
print(i+n0,">>>>>>>",colname,":value=",df_BRBNT.loc[i+n0,colname])
dropindex.append(i+n0)
df_BRBNT.loc[i+n0,colname]=ewm_mean[i+n0]
#df_BRBNT=df_BRBNT.drop(index=[i+n0])
#i+=1
#print("dropindex=",dropindex)
#df_BRBNT=df_BRBNT.drop(index=dropindex)
#list_col=df_BRBNT.loc[n0:n1,colname]
#df_BRBNT1= df_BRBNT.query('@lower <df_BRBNT.loc[n0:n1,colname] <= @upper')
#df_BRBNT1=df_BRBNT[(df_BRBNT.loc[n0:n1+1,colname] > upper) or (df_BRBNT.loc[n0:n1+1,colname] < lower)]
"""
#補間パラメータレンジ
dRange=df_BRBNT.loc[n0:n1,colname]#補間データの範囲List
iTOWB0Range=df_BRBNT.loc[n0:n1,"iTOW_B0"]#ゼロカウンタの範囲List
iTOWB_start=int(df_BRBNT.loc[n0,"iTOW_B"])#iTOW_B開始値
x_observed=iTOWB0Range.to_numpy()#X軸はiTOW_Bのゼロカウンタの範囲Listをdarry
y_observed=dRange.to_numpy()#Y軸は、dRangeをYデータとしてdarry
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)#補間で埋める時間darry
#iTOW_Bの補間で埋める時間darry
iTOWB_kizami=np.linspace(x_observed[0]+iTOW_B_init, x_observed[lastn]+iTOW_B_init, kizami)
# dataをip6:２次スプライン補間計算
for method_name, method in [ ip6]:#ip6:２次数スプライン１個で固定
#print(method_name)
fig_idx = 0#グラフ番号初期
txt=":"+method_name+" Range="+str(n0)+"-"+str(n1)#グラフタイトル
fitted_curve = method(x_observed, y_observed)#２次スプラインで補間したカーブ
fit_result=fitted_curve(x_latent)#
list_iTOW_B=iTOWB_kizami.tolist()
list_interP = fit_result.tolist()
#------------------interPの外れ値除去-------------------------
#fig = plt.figure()
# add_subplot()でグラフを描画する領域を追加する．引数は行，列，場所
#ax1 = fig.add_subplot(2,1, 1,title=colname+"_ITP")
#ax2 = fig.add_subplot(2,1, 2,title=colname+"_BRBNT") #ax1.twinx() # x軸を共有
#ax3 = fig.add_subplot(3,1, 3,title=colname+"_BRBNT1") #ax1.twinx() # x軸を共有
#ax1.set_xlabel('iTOWB_kizami') #x軸ラベル
#ax1.plot(iTOWB_kizami,fit_result, color="blue", label=colname+"_ITP")
#ax2.plot(x_observed,y_observed, color="red", label=colname+"_BRBNT")
#ax3.plot(x_observed,df_BRBNT1.loc[n0:n1,colname], color="red", label=colname+"_BRBNT1")
#plt.scatter(iTOWB_kizami,fit_result)
#plt.scatter(x_observed,y_observed)
#plt.title(colname)
#plt.show()
#DF用のlistは、横並びでappendさせて、最後に転置してDF変換df_interPDM
#if i==0:
#list_interPDM.append(list_iTOW_B)
#print("list_iTOW_B append i=",i,"kaisu=",kaisu)
#list_iTOW_B.clear()
list_interPDM.append(list_interP)
#print("kaisu=",str(kaisu),"list_interPDM=",list_interPDM)
#print("kaisu=",kaisu,":n0,n1=",n0,n1,"xobserved[0],[lastn]",x_observed[0],x_observed[lastn],"+iTOW_B_init",x_observed[0]+iTOW_B_init)
kaisu+=3#kasiuを３個飛びで４STEP
df_interPDM = pd.DataFrame(list_interPDM)#df作成
print("df_interPDM PreTranspose:",str(kaisu),df_interPDM.shape)
df_interPDM=df_interPDM.transpose()#df転置
print("df_interPDM PostTranspose:",str(kaisu),df_interPDM.shape)
df_PDMdes=df_interPDM.describe()
df_PDMdes.columns=list_call#
df_PDMdes.index=['count', 'mean','std','min','25%','50%','75%','max']
#df_PDMdes.reset_index(drop=True, inplace=True)
print("df_PDMdes shape:",str(kaisu),df_PDMdes.shape)
df_PDMdesAll = pd.concat([df_PDMdesAll, df_PDMdes],axis=0)
#df_PDMdes=0
#df_PDMdesAll=
#ヘッダ名追加　先頭に"iTOW_B"
#list_call.insert(0, "iTOW_B")
print("list_call len=",len(list_call))
df_interPDM.columns=list_call#
#print("df_interPDM 0=",df_interPDM.iloc[0,0])
print("df_interPDM=",df_interPDM)
df_interPDM.to_csv(r'C:/RTK_Log/hokan/interPDMRTK_'+str(kaisu)+"_"+ basename, index=False) # CSV sav
df_new=bno_add(df_interPDM,df_BNOfull)
df_new.to_csv(r'C:/RTK_Log/hokan/sITPBNO_'+str(kaisu)+"_"+ basename, index=False) # CSV sav
list_interPDM.clear()
#===============================================================
df_PDMdesAll.to_csv(r'C:/RTK_Log/hokan/interPDMdesAll_'+"_"+ basename, index=True) # CSV sav
#==========補間済みのファイル群を一本のAllファイルにまとめる==============
fnam="C:/RTK_Log/hokan/interPDMRTK_3_"+ basename
ITPAll_make(fnam)
#print("補間　個別ファイル　Allファイル生成完了")
elif v=='s' :# 旧interPolate　補間 計算　SAVE CSV+ TEST035で headMot追加============================================================================================
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"]
gSRange=df_BRBNT.loc[n0:n1,"gSpeed_B"]
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_gS=gSRange.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_gS = method(x_observed, y_observed_gS)
fitted_curve_hD=method(x_observed, y_observed_hD)
fit_result_gS=fitted_curve_gS(x_latent)
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_gS=fit_result_gS.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)
list_interPDM.append(list_iTOW_B)
list_interPDM.append(list_interP_hD)
list_interPDM.append(list_interP)
list_interPDM.append(list_interP_gS)
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_interPDM = pd.DataFrame(list_interPDM)
df_interPDM=df_interPDM.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
df_interPDM.to_csv(r'C:/RTK_Log/interPDM_' + basename, index=False) # CSV sav
# if"s" End====================================================================================================
elif v=='ball' :#RTK補間データITPAll_interPDMRTKをBNOと同期合体　BNOファイルを読み込ん
print("**********Had BRBNT FILE read ?**************")
#<<<<BNO生データファイル読み込み-----------------------------------------------------
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)
basenameBNO = os.path.basename(filename)
df_BNOfull = pd.read_csv(filename, low_memory=True)
df_BNOfull.columns=["bnitow","yaw","pitch","roll","ax","ay","az"]#コラムヘッダ名追加
print("df_BNOfull:",df_BNOfull.shape,basenameBNO)
print(df_BNOfull)
#<<<<<ITPAll ファイル読み込み-------------------------------------------------------
print(">>>>SELECT ITPAll FILE")#<<<<<<<ITPAll ファイル読み込み
fTyp = [("ITPAllデータファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
basenamePDM = os.path.basename(filename)
#BRBNflag = "BRBN" in basename
#df
df_ITPAll = pd.read_csv(filename, low_memory=True) #interPDM File　READ
print("df_ITPAll:",df_ITPAll.shape,basenamePDM)
#BNITOWの整頓
iTOW_B_start=int(df_BRBNT.loc[diffN[0],"iTOW_B"])#BRBNTのdiffN開始位置のiTOW_B
print(df_ITPAll)
data=[]
data=df_BNOfull.iloc[:,0]#bnitow　List
value=iTOW_B_start
aidx=idx_of_the_nearest(data, value)#data"bnitow"とiTOW_B_startに近い値aidx
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
i=0
data=[]
coln=len(df_ITPAll.columns)
# iTOW_B{0},heaDing{1},headMot_B(2),gSpeed_B(3),iTOW_B(4),heaDing(5),headMot_B(6),gSpeed_B(7)・・・
#for i in range(0,coln,4):
df_ITPset= pd.DataFrame()
df_BNOset= pd.DataFrame()
df_ITPBNO =pd.DataFrame()
st=0
#df_ITPset=df_interPDM.iloc[:,[st,st+1,st+2,st+3]]
df_ITPset=df_ITPAll
#print("==========st=",st,"df_ITPAll.iloc[0,st:st+1]=",df_ITPAll.iloc[0,st],df_ITPAll.iloc[0,st+1],df_ITPAll.iloc[0,st+2],df_ITPAll.iloc[0,st+3])
print("df_ITPset=",df_ITPset)
#iTOW_B_start=int(df_BRBNT.loc[diffN[i],"iTOW_B"])
ITP_start=df_ITPAll.loc[0,"iTOW_B"]
#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)#合体
print("df_ITPBNO",df_ITPBNO,"BNOser[0:ilen,2]=",df_BNOset.iloc[0:ilen,2])
#sa = df_ITPset.iloc[0:ilen,1].sub(df_BNOset.iloc[0:ilen,2])
#df_ITPBNO=df_ITPBNO.set_axis(['iTOW_B', 'heaDing','headMot_B','gSpeed_B','bnIndex' ,'bniTOW','yaw','pitch','roll','ax','ay','az'], axis='columns')
#print("columns=",df_ITPBNO.columns
# ---Heading角　補正
df_ITPBNO["Heading180"] = df_ITPBNO.relPosHeading_R.multiply(0.00001) + 180
df_ITPBNO["Heading360"] = np.where(df_ITPBNO["Heading180"] > 360, df_ITPBNO["Heading180"] - 360, df_ITPBNO["Heading180"])
#sa = df_ITPset.loc[0:ilen,"relPosHeading_R"].sub(df_BNOset.iloc[0:ilen,"yaw"])
sa = df_ITPBNO.loc[0:ilen,"Heading360"].sub(df_BNOset.loc[0:ilen,"yaw"])
df_ITPBNO["H-Ysa360"]=sa
print("df_ITPBNO=",df_ITPBNO)
#print("sa=",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,"H-Ysa360"]
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()
elif v=='b' :#RTK補間データをBNOと同期合体　BNOファイルを読み込んでdf_interPDMと合体
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_BNOfull作成
df_BNOfull = pd.read_csv(filename, low_memory=True)
print("df_BNOfull:",df_BNOfull.shape,basenameBNO)
print(df_BNOfull)
print(">>>>SELECT interPDMRTK FILE")
fTyp = [("interPDMRTKデータファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
basenamePDM = os.path.basename(filename)
#BRBNflag = "BRBN" in basename
#df
df_interPDM = pd.read_csv(filename, low_memory=True) #interPDM File　READ
print("df_interPDM:",df_interPDM.shape,basenamePDM)
#BNITOWの整頓
iTOW_B_start=int(df_BRBNT.loc[diffN[0],"iTOW_B"])
print(df_interPDM)
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_interPDM.columns)
# iTOW_B{0},heaDing{1},headMot_B(2),gSpeed_B(3),iTOW_B(4),heaDing(5),headMot_B(6),gSpeed_B(7)・・・
for i in range(0,coln,4):
df_ITPset= pd.DataFrame()
df_BNOset= pd.DataFrame()
df_ITPBNO =pd.DataFrame()
st=i
#df_ITPset=df_interPDM.iloc[:,[st,st+1,st+2,st+3]]
df_ITPset=df_interPDM
print("==========st=",st,"df_interPDM.iloc[0,st:st+1]=",df_interPDM.iloc[0,st],df_interPDM.iloc[0,st+1],df_interPDM.iloc[0,st+2],df_interPDM.iloc[0,st+3])
print("df_ITPset=",df_ITPset)
#iTOW_B_start=int(df_BRBNT.loc[diffN[i],"iTOW_B"])
ITP_start=df_interPDM.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)#合体
print("df_ITPBNO",df_ITPBNO,"BNOser[0:ilen,2]=",df_BNOset.iloc[0:ilen,2])
sa = df_ITPset.iloc[0:ilen,1].sub(df_BNOset.iloc[0:ilen,2])
print("sa=",sa)
df_ITPBNO=df_ITPBNO.set_axis(['iTOW_B', 'heaDing','headMot_B','gSpeed_B','bnIndex' ,'bniTOW','yaw','pitch','roll','ax','ay','az'], axis='columns')
#print("columns=",df_ITPBNO.columns
df_ITPBNO["H-Ysa360"]=sa
print("df_ITPBNO=",df_ITPBNO)
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,"H-Ysa360"]
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()
elif v=='a' :#ITPBN用ペアプロット
print("Anlysys")
print(">>>> SELECT ITPBNO FILE")
root = tkinter.Tk()
root.withdraw()
iDir = r'C:/RTK_Log/hokan'
fTyp = [("BNOデータファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
# filenameからbasenameをとりだしてファイル種類区別
basenameITP = os.path.basename(filename)
# BRBNflag = "BRBN" in basename
#df
df_ITPBNO = pd.read_csv(filename, low_memory=True)
print("df_ITPBNO:",df_ITPBNO.shape,basenameITP)
df_ITPBNO["vect"]=df_ITPBNO["ax"]**2+df_ITPBNO["ay"]**2+df_ITPBNO["az"]**2
print(df_ITPBNO)
#==========difN作成=========================================
difN=peaks10(df_ITPBNO)
print("difN=",difN)
#=======================Input Range=============================================
print(">>>> SELECT Plot Range[0-",len(df_ITPBNO),"]")
selgrph=""
while selgrph!="e":
rngstr= input(" keyin Range startrow-endrow -h Hue or -n normal or -e exit =>ENTER")
list_range=[]
list_range=rngstr.split('-')
sttrow=int(list_range[0])
endrow=int(list_range[1])
selgrph=list_range[2]
if selgrph=='e':
break
print("start row=",sttrow,"end row=",endrow)
#=============================================================================
df_pplot=df_ITPBNO.loc[sttrow:endrow,["iTOW_B","Heading360","headMot360","yaw","pitch","roll","H-Ysa360"]]
df_pplot=df_pplot.rename(columns={'H-Ysa360': 'HYsa360'})
#print("df_pplot.columns=",df_pplot.columns.values)
#=====外れ値除去 　Heading360 H-Ysa360
#thr1 = df_pplot["Heading360"].quantile(0.95)
#thr2 = df_pplot["Heading360"].quantile(0.05)
#df_pplot = df_pplot.query('@thr2 < Heading360 <= @thr1')
thr3 = df_pplot["HYsa360"].quantile(0.95)
thr4 = df_pplot["HYsa360"].quantile(0.05)
print("thr3=",thr3)
print("thr4=",thr4)
df_pplot = df_pplot.query('@thr4 <HYsa360<= @thr3')
#print("df_pplot=",df_pplot)
#fig=plt.figure(figsize=(4,4))
if selgrph=="h":
g=sns.pairplot(df_pplot,hue="HYsa360")#,palette={"H-Ysa360":'r",ed',"headDing":'blue',"yaw":'green'})#seabornでpairプロットデータ作成してsnsオブジェクト作成
g.figure.suptitle(rngstr+basenameITP, y=0.995,size=18, weight=2, color="red") # y= some height>1
g.savefig('C:/RTK_Log/hokan/ITPparirplot_Hue_'+basenameITP+rngstr+'.png')
plt.show()
else:
g=sns.pairplot(df_pplot)
g.figure.suptitle(rngstr+basenameITP, y=0.995,size=18, weight=2, color="red")
g.savefig('C:/RTK_Log/hokan/ITPparirplot_'+basenameITP+rngstr+'.png')
plt.show()
#sns.pairplot(df_ITPBNO).savefig('C:/RTK_Log/hokan/ITPparirplot'+basenameITP+'.png')
elif v=='pp' :#ITPBN用ペアプロット
print("Anlysys")
list_result=[]# SVM result acc f1
print(">>>> SELECT interPDM_RTK FILE")
root = tkinter.Tk()
root.withdraw()
iDir = r'C:/RTK_Log/hokan'
fTyp = [("BNOデータファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
# filenameからbasenameをとりだしてファイル種類区別
basenameITPRTK = os.path.basename(filename)
# BRBNflag = "BRBN" in basename
#df
df_ITPRTK = pd.read_csv(filename, low_memory=True)
print("df_ITPRTK:",df_ITPRTK.shape,basenameITPRTK,df_ITPRTK)
#df_pplot_B=df_ITPRTK.loc[:,df_ITPRTK.columns.str.endswith('_B')]#サフィックス_Bを抽出
df_pplot=df_ITPRTK.loc[:,["headMot_B","gSpeed_B","velN_B","velE_B","velD_B","relPosN_B","relPosE_B"]]
#g=sns.pairplot(df_pplot,hue="H-Ysa360")#,palette={"H-Ysa360":'r",ed',"headDing":'blue',"yaw":'green'})#seabornでpairプロットデータ作成してsnsオブジェクト作成
g=sns.pairplot(df_pplot)
#g.fig.suptitle(basenameITP, y=0.995,size=18, weight=2, color="red") # y= some height>1
g.savefig('C:/RTK_Log/hokan/ITPparirplot'+basenameITPRTK+'.png')
plt.show()
#************************SVM****************************************************************
elif v=='m' :#SVM　モデル作成と予測　検証
print(">>>>>>>>>>>>>SELECT sITPBNO FILENAME ")
list_result=[]
root = tkinter.Tk()
root.withdraw()
iDir = r'C:/RTK_Log/hokan'
fTyp = [("BNOデータファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
basenameITP = os.path.basename(filename)
instr=''
colist_org=["Heading360","headMot360","headMotdiff","yaw","roll","H-Ysa360"]
listN_org=['3','6','9','12','15','18']
df=sITP_dfMake(filename,listN_org,colist_org,"org")
print("ORG DF:df=",df)
while instr!='e':
#^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
#SVM用正規化df作成ブロック　ここで書き換えて自動作成　新カラムは、sITP_dfMake内で作成
instr= input(" >>>>>>SELECT DataNo tr':Training 'te';Test-No1-No2-No3- KEY IN last '")
#>>train
colist_train=["headMotdiff","roll"]
listN_train=['3','6','9','12','15','18']
df_trains=sITP_dfMake(filename,listN_train,colist_train,"train")
#>>test
colist_test=["headMotdiff","roll"]
listN_test=['24','27']
df_tests=sITP_dfMake(filename,listN_test,colist_test,"test")
#>>Training target
colist_target=["H-Ysa360"]
listN_target=listN_train
df_target=sITP_dfMake(filename,listN_target,colist_target,"target")
#df_target ランク分け
rankN=['1','5']
values=[[0,4],[4,20]]#0<d<4 4<d<20
colname="H-Ysa360"
df_targetR=tRank_make(df_target,rankN,values,colname)
#>>Testing target
colist_ttarget=["H-Ysa360"]
listN_ttarget=['24','27']
df_ttarget=sITP_dfMake(filename,listN_ttarget,colist_ttarget,"target")
#df_ttarget rank分け
rankN=['1','5']
values=[[0,4],[4,20]]#0<d<4 4<d<20
colname="H-Ysa360"
df_ttargetR=tRank_make(df_ttarget,rankN,values,colname)
#特徴量DFをnumpyArrayに変換==========================================
df_Xtrain=df_trains
df_Xtest=df_tests
#print("df_Xtrain=",df_Xtrain)
Xn=df_Xtrain.to_numpy()
Xte=df_Xtest.to_numpy()
print("len(Xn),Xn",len(Xn),Xn)
print("len(Xte),Xte",len(Xte),Xte)
#======================================================================================================
# SVM start
nc=len(colist_train)
X = np.array(Xn).reshape(-1,nc)#全行ｘ6列を1行にまとめる
#y=np.array(y_targetStr).reshape(-1,1)
#y=np.array(list_target)
y=df_targetR.to_numpy()
print("X shape X=",X.shape,X)
print("y shape y=",y.shape,y)
#model = SVC(kernel = 'linear', C=1, gamma=1) #学習モデルのパラメータを指定
#from sklearn.svm import SVC
# 線形SVMのインスタンスを生成
model = SVC(kernel='linear', random_state=None)#Lineat
#***********TrainingデータX,yで学習開始*************************************************************
model.fit(X,y) #学習を行う
with open('model.sav', 'wb') as fp_model: # 学習モデルを保存するためのファイルを開く
pickle.dump(model, fp_model) # モデルを保存
print('Model created.')
# Trainingデータのモデルフィット==================================================
from sklearn.metrics import accuracy_score, f1_score
y_pred = model.predict(X)
print("y_pred",len(y_pred))
acc=accuracy_score(y, y_pred)#TargetデータとTrainingデータモデルフィットの比較評価点
accstr='{:>10.2%}'.format(acc)#acc
f1=f1_score(y, y_pred, average="macro")#F1
f1str='{:>10.2%}'.format(f1)
# Testデータのモデルフィット==================================================
ytest_pred = model.predict(Xte)
y=df_ttargetR.to_numpy()
print("ytest_pred",len(ytest_pred))
acctest=accuracy_score(y, ytest_pred)#TargetデータとTrainingデータモデルフィットの比較評価点
accteststr='{:>10.2%}'.format(acctest)#acc
f1test=f1_score(y, ytest_pred, average="macro")#F1
f1teststr='{:>10.2%}'.format(f1test)
print("*************Training RESULT PRINT*****************")
print("FILE:",filename)
print("TrainingDataNo:",instr)
rtxt="Training:"+filename+","+instr+","+accstr+","+f1str
print(rtxt)
list_result.append(rtxt)
print("*************Test RESULT PRINT*****************")
rtesttxt="Test:"+filename+","+instr+","+accteststr+","+f1teststr
print(rtesttxt)
list_result.append(rtesttxt)
#pplot==============================================
#print("df columns=",df.columns)
df_pplot=df[["Heading360","headMot360","headMotdiff","yaw","roll","H-Ysa360"]]
print("pplot=",df_pplot)
pairplot(df_pplot)#相関付きペアプロット
plt.show()
#以下普通のペアプロット
g=sns.pairplot(df_pplot,hue="H-Ysa360")#,palette={"H-Ysa360":'r",ed',"headDing":'blue',"yaw":'green'})#seabornでpairプロットデータ作成してsnsオブジェクト作成
g.figure.suptitle("2D_"+rtxt+"_HUE", y=0.995,size=12, weight=2, color="red") # y= some height>1
#g.figure.suptitle(instr+basenameITP+"HUE", y=0.995,size=18, weight=2, color="red") # y= some height>1
g.savefig('C:/RTK_Log/hokan/ITPparirplot_'+basenameITP+instr+'_HUE.png')
plt.show()
g=sns.pairplot(df_pplot)#,palette={"H-Ysa360":'r",ed',"headDing":'blue',"yaw":'green'})#seabornでpairプロットデータ作成してsnsオブジェクト作成
g.figure.suptitle("2D_"+rtxt,y=0.995,size=12, weight=2, color="red") # y= some height>1
g.savefig('C:/RTK_Log/hokan/ITPparirplot_'+basenameITP+instr+'.png')
plt.show()
#df_nm1000.to_csv(r'C:/RTK_Log/hokan/SVM_yPred_'+basename, index=False) # CSV sav
# CSV読み込みして散布図プロット
#df = pd.read_csv("imu.csv", sep=",", header=None, names=['gnum', 'gyX', 'gyY', 'gyZ'])
#df = pd.read_csv("imu.csv", sep=",", header=None, names=['gnum', 'acX', 'acY', 'acZ'])
elif v=='ppr' :#sITP用相関解析ペアプロット
print()
print(">>>>>>>>>>>>>SELECT sITPBNO FILENAME ")
list_result=[]
root = tkinter.Tk()
root.withdraw()
iDir = r'C:/RTK_Log/hokan'
fTyp = [("BNOデータファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
basenameITP = os.path.basename(filename)
instr=''
colist_org=["iTOW_B","Heading360","headMot360","headMotdiff","yaw","roll","H-Ysa360","skid","skidyaw"]
listN_org=['3','6','9','12','15','18']#2024-06-12-10:06
#listN_org=['21','24','27','30','33','36']#2024-06-12-10:20
#listN_org=['39','42','45','48','51']#2024-06-12-10:30
#listN_org=['24','27','33','39']
#listN_org=['42','45','48','51']
#listN_org=['6','9']
listN_org=['3','6','9','12','15','18','21','24','27','30','33','36','42','45','48','51']#2024-06-13-11:08
listNstr= ','.join(listN_org)
df=sITP_dfMake(filename,listN_org,colist_org,"org")
print("ORG DF:df=",df)
df.to_csv(r'C:/RTK_Log/hokan/sITP_ORG_'+listNstr +"_" +basenameITP, index=True) # CSV sav
#相関解析p
from sklearn import linear_model
# 回帰分析を行うときのおまじない
reg = linear_model.LinearRegression()
# sklearnを使う時は、numpy形式に変換する
x = df.loc[:,"roll" ].to_numpy()
y = df.loc[:,"skid"].to_numpy()
X=x.reshape(-1,1)
Y=y.reshape(-1,1)
# 回帰分析を実行
reg.fit(X, Y)
# 結果の出力
print('回帰係数：', reg.coef_)
print('切片：', reg.intercept_)
print('傾き=',reg.coef_, '切片=', reg.intercept_)
print('決定係数 R^2', reg.score(X, Y))
#pplot==============================================
#print("df columns=",df.columns)
#df_pplot=df[["Heading360","headMot360","headMotdiff","yaw","roll","H-Ysa360","skid","skidyaw"]]
df_pplot=df[colist_org]
print("pplot=",df_pplot)
supname=listNstr+basenameITP
pairplot(df_pplot,supname)#相関付きペアプロット
plt.show()
#以下普通のペアプロット
g=sns.pairplot(df_pplot,hue="skid")#,palette={"H-Ysa360":'r",ed',"headDing":'blue',"yaw":'green'})#seabornでpairプロットデータ作成してsnsオブジェクト作成
g.figure.suptitle(listNstr+basenameITP+"_HUE", y=0.995,size=12, weight=2, color="red") # y= some height>1
#g.figure.suptitle(instr+basenameITP+"HUE", y=0.995,size=18, weight=2, color="red") # y= some height>1
g.savefig('C:/RTK_Log/hokan/ppr_'+listNstr+basenameITP+instr+'_HUE.png')
plt.show()
g=sns.pairplot(df_pplot)#,palette={"H-Ysa360":'r",ed',"headDing":'blue',"yaw":'green'})#seabornでpairプロットデータ作成してsnsオブジェクト作成
g.figure.suptitle(listNstr+basenameITP,y=0.995,size=12, weight=2, color="red") # y= some height>1
g.savefig('C:/RTK_Log/hokan/ppr_'+listNstr+basenameITP+instr+'.png')
plt.show()
elif v=='pph' :#yaw H-Ysa相関式で補正かける
print(">>>>>>>>>>>>>SELECT sITP_ORG FILENAME ")
list_result=[]
root = tkinter.Tk()
root.withdraw()
iDir = r'C:/RTK_Log/hokan'
fTyp = [("sITPデータファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
basenameITP = os.path.basename(filename)
df_pph = pd.read_csv( filename, low_memory=True)
print(df_pph.shape)
cut_Lturn(df_pph,basenameITP)
def cut_Lturn(df,bnm):
g=220
print("Turn L peak to epak cut H-Ysa360 roll相関")
#df["rolldiff"]=df["roll"].diff()
#print("rolldiff",df["rolldiff"])
df["roll_mult"] = df['roll'] * df['roll'].shift(1)
df_rollzero=df[df["roll_mult"]<0]
list_zeroindex=df_rollzero.index
print("df_rollzero=",df_rollzero)
print("list_zeroindex=",list_zeroindex)
#roll の　diff()
df["rolldiff"]=df["roll"].diff()*10
#補正値生成　H-Ysa360 に　rollｘ０．５を足す、
#H-Ysa360=Heading360 - yaw
df["yaw56"]=df["yaw"]+g
df["skidyawg"]=df["skidyaw"]-g
df["H-Ysa36056"]=df["yaw56"]-df["Heading360"]
df["H-Yhosei"]=df["H-Ysa36056"]+df["roll"]*0.25
df["yawhosei"]=df["yaw56"]-df["H-Ysa36056"]
df["headMotMA3"]=df["headMot360"].rolling(10).mean()
#--
fig = plt.figure()
# add_subplot()でグラフを描画する領域を追加する．引数は行，列，場所
ax1 = fig.add_subplot(1,1, 1)
ax2 = ax1.twinx()
ax1.grid(which="both")
ax1.minorticks_on()
ax2.grid()
ax1.set_ylim(0, 240)
ax2.set_ylim(-90, 50)
x0=np.arange(len(df))
x1=np.array(list_zeroindex)
y0=df["roll"].to_numpy()
y1=(df_rollzero["roll"]).to_numpy()
y2=(df["yaw56"]).to_numpy()
y3=df["H-Ysa360"].to_numpy()
y4=df["H-Ysa36056"].to_numpy()
y5=df["yawhosei"].to_numpy()
y6=df["Heading360"].to_numpy()
y7=(df["skidyawg"]).to_numpy()
y8=df["headMotMA3"].to_numpy()
y9=(df["ax"]).to_numpy()
#y10=(df["ay"]).to_numpy()
ax2.plot(x0,y0,color="blue",label="roll")
ax2.plot(x0,y7,color="aqua",label="skidyaw")
ax1.plot(x0,y2,color="green",label="yaw56")
#ax2.plot(x0,y3,color="red",label="H-Ysa360")
ax2.plot(x0,y4,color="red",label="H-Ysa36056")
#ax1.plot(x0,y5,color="black",label="yawhosei")
ax1.plot(x0,y6,color="orangered",label="Heading360")
ax1.plot(x0,y8,color="deeppink",label="headMot360")
#ax2.plot(x0,y9,color="pink",label="ax*10")
#ax2.plot(x0,y10,color="pink",label="ay*10")
#grid
ax2.hlines(y=0,xmin=0, xmax=len(df),color="blue")
ax2.hlines(y=5,xmin=0, xmax=len(df),color="black")
ax2.hlines(y=-5,xmin=0, xmax=len(df),color="black")
ax1.hlines(y=56, xmin=0, xmax=len(df),color="red")
ax1.hlines(y=0, xmin=0, xmax=len(df),color="blue")
ax1.vlines(x=x1.tolist(),ymin=0,ymax=360,color="blue")
ax1.plot(x1,y1,color="red",marker = 'o',linewidth=0,label="roll_zero")
#plt.legend()
ax1.legend(loc="upper left")
ax2.legend(loc="upper right")
fig.suptitle(bnm, y=0.995,size=10, weight=2, color="red")
fig.savefig("C:/RTK_LOG/hokan/pph_time_"+bnm+"_.png")
plt.show()
df.to_csv(r'C:/RTK_Log/hokan/pph_time_'+bnm+'.csv', index=False) # CSV sav
return df
def corr_func(x, y, **kws):
mask = ~np.logical_or(np.isnan(x), np.isnan(y))
x, y = x[mask], y[mask]
r, _ = stats.pearsonr(x, y)
ax = plt.gca()
ax.annotate("r = {:.3f}".format(r),
xy=(.2, .5),
xycoords=ax.transAxes,
size=16)
def pairplot(df,basenam):
g = sns.PairGrid(df, height=1.6, dropna=False)
g.map_diag(draw_hist)
g.map_upper(sns.regplot, scatter_kws={"s": 8}, line_kws={"color": "r"})
g.map_lower(corr_func)
g.figure.suptitle("2D_"+basenam,y=0.995,size=12, weight=2, color="red")
g.savefig('C:/RTK_Log/hokan/ITPreg'+basenam+'.png')
def draw_hist(x, **kws):
plt.hist(x[~np.isnan(x)])
main()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment