Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/STA25_NewTurnCordinate_STA24_All_LPF_28.py Secret

Created October 7, 2024 01:16
Show Gist options
  • Save dj1711572002/9fe3cc900bfee46f5906c2b3a87c2a00 to your computer and use it in GitHub Desktop.
Save dj1711572002/9fe3cc900bfee46f5906c2b3a87c2a00 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
STA25_NewTurnCordinate_STA24_All_LPF_28.py
import numpy as np
import itertools
import seaborn as sns
import matplotlib.pyplot as plt
from scipy import interpolate
from scipy.signal import find_peaks
from scipy import signal
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
from os.path import dirname, basename
from os.path import join
import glob
from datetime import datetime, date, timedelta
import re
import sys
import pickle
import math
import cmath
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 peaksHokancol(df,colname):#Rピーク検索して補間　範囲内MAXでピーク検出
srange=40#個数分の範囲でピーク探す
prange=100#100度以上差がでる
av=df[colname].mean()
sd=df[colname].std()
df["coldif"]=df[colname]-av#
df["colmult"]=df['coldif'] * df['coldif'].shift(1)
list_zero=df[df["colmult"]<0].index
#print(colname,":list_zero=",list_zero)
maxidx=[]
for i in range(0,len(list_zero)-1):
n0=list_zero[i]
n1=list_zero[i+1]
val=df.loc[n0:n1,colname].max()
val0=df.loc[n0,colname]
val1=df.loc[n1,colname]
print(">>>>",colname,"val0=",str(val0),"val1=",str(val1),"valmax=",str(val),"av+sd=",str(av+sd))
if val-val0>10:
maxidx.append(df.loc[n0:n1,colname].idxmax())
#print(colname,":maxidx=",maxidx)
#print(colname,":maxvalue=",df.loc[maxidx,colname])
return maxidx
def peak_Maxmin(df,colname):
srange=40#個数分の範囲でピーク探す
prange=100#100度以上差がでる
av=df[colname].mean()
sd=df[colname].std()
df["coldif"]=df[colname]-av#
df["colmult"]=df['coldif'] * df['coldif'].shift(1)
list_zero=df[df["colmult"]<0].index
#print(colname,":list_zero=",list_zero)
result=[[]]
i=0
while i<len(list_zero)-3:
flag0=0
flag1=0
n0=list_zero[i]
n1=list_zero[i+1]
n2=list_zero[i+2]
print("i=",i,"n0,n1,n2=",n0,n1,n2)
center=int((n0+n1)/2) #センターライン位置
val0=df.loc[n0,colname] #平均値
valc=df.loc[center,colname]#中心高さ
#print(">>>>",colname,"val0=",str(val0),"val1=",str(val1),"valmax=",str(valmax),"valmin=",valmin,"av+sd=",str(av+sd))
if valc-val0>10:#上に凸
valmax=df.loc[n0:n1,colname].max()
maxidx=df.loc[n0:n1,colname].idxmax()
flag0=1
#elif valc-val0<-10:#下に凸
valmin=df.loc[n1:n2,colname].min()
minidx=df.loc[n1:n2,colname].idxmin()
flag1=1
#if flag0==1 and flag1==1:
list_result=[n0,n1,n2,maxidx,minidx]
print(i,":list_result",list_result)
result.append(list_result)
flag0=0
flag1=0
print("i=",i)
i+=2
else:
i+=1 #print(colname,":maxidx=",maxidx)
df_result=pd.DataFrame(result)
df_result = df_result.drop(0)
df_result=df_result.reset_index()
df_result.columns=["idx","n0","n1","n2","maxidx","minidx"]
print(colname,"df_result=",df_result)
return df_result
#==========================================================================
# 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)
#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
if flag=="ng":#shinpukuグラフ無し
df1_std=df0
else:
#=============振幅追加====================
Shinpuku(df1,fn,listN)
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
#*****************ITP個別ファイルにBNO列追加　SkiOn BootsOn両方合体******************************************************
def bno_addT(df_ITP,df_BNOfull,df_BNOfullT):
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 butterworth_filter(time_series, delta_t, f_cut, order):
b, a = signal.butter(order, f_cut, btype='low', fs=1/delta_t)
return signal.filtfilt(b, a, time_series)
def LPF(df,coln):
print("df_colnにLowPassFilter処理して、df_coln_fというコラムをつけて戻す")
#------------butter worse--------------------------
# パラメータ設定
sample_freq = 100
cutoff_freq = 1#1hz固定
filter_order = 4
sec = len(df)*0.01#dfの時間
# ----colnをフィルタ
t = np.linspace(0, sec , int(sample_freq * sec))
y=df.loc[:,coln].to_numpy()
sos = signal.butter(filter_order, cutoff_freq, 'lowpass', output='sos', fs=sample_freq)
yf = signal.sosfiltfilt(sos, y)
print(yf)
new_coln=coln+"_f"
df[new_coln]=pd.DataFrame(yf)
return df
def CircleFitting(x,y):
"""最小二乗法による円フィッティングをする関数
input: x,y 円フィッティングする点群
output cxe 中心x座標
cye 中心y座標
re 半径
参考
一般式による最小二乗法（円の最小二乗法）　画像処理ソリューション
http://imagingsolution.blog107.fc2.com/blog-entry-16.html
"""
sumx = sum(x)
sumy = sum(y)
sumx2 = sum([ix ** 2 for ix in x])
sumy2 = sum([iy ** 2 for iy in y])
sumxy = sum([ix * iy for (ix,iy) in zip(x,y)])
F = np.array([[sumx2,sumxy,sumx],
[sumxy,sumy2,sumy],
[sumx,sumy,len(x)]])
G = np.array([[-sum([ix ** 3 + ix*iy **2 for (ix,iy) in zip(x,y)])],
[-sum([ix ** 2 *iy + iy **3 for (ix,iy) in zip(x,y)])],
[-sum([ix ** 2 + iy **2 for (ix,iy) in zip(x,y)])]])
T=np.linalg.pinv(F).dot(G)
cxe=float(T[0]/-2)
cye=float(T[1]/-2)
re=math.sqrt(cxe**2+cye**2-T[2])
#print (cxe,cye,re)
return (cxe,cye,re)
"""
#if __name__ == '__main__':
#import matplotlib.pyplot as plt
#Unit Test
#推定する円パラメータ
cx=4; #中心x
cy=10; #中心y
r=30; #半径
#円の点群の擬似情報
plt.figure()
x=range(-10,10);
y=[]
for xt in x:
y.append(cy+math.sqrt(r**2-(xt-cx)**2))
#円フィッティング
(cxe,cye,re)=CircleFitting(x,y)
#円描画
theta=np.arange(0,2*math.pi,0.1)
xe=[]
ye=[]
for itheta in theta:
xe.append(re*math.cos(itheta)+cxe)
ye.append(re*math.sin(itheta)+cye)
xe.append(xe[0])
ye.append(ye[0])
plt.plot(x,y,"ob",label="raw data")
plt.plot(xe,ye,"-r",label="estimated")
plt.plot(cx,cy,"xb",label="center")
plt.axis("equal")
plt.grid(True)
plt.legend()
plt.show()
"""
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=[]
BRBN_flag=0#BRBNファイル=1かBRBNTファイル=0か区別
ReadFile="NO"
while(True):#キーボード入力で処理機能選択
#===READ FILE
#print("kaisu=",kaisu)
print("<<<<PUSH KEY::r:read BRBNT s:readBNO ,p:plot c:CircularLSQM e=EXIT>>>> Existing ReadFile=",ReadFile)
v= input(" keyin =>ENTER")
#**************KEY BRANCHES************************************************
if v=='r':#>>>>BRBNとかBRBNTファイルを読む-------------------------------------------------
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をとりだしてファイル種類区別
print(">>>>>>READ FILE;",filename)
basename = os.path.basename(filename)
ReadFile=basename
BRBNflag = "BRBN" in basename
if "BRBNT" in basename:
BRBN_flag=0
else:
BRBN_flag=1
#df
df_BRBNT = pd.read_csv(filename, low_memory=True)
cols=df_BRBNT.columns
print(cols)
if "diffzero" in cols:
print()
else:
df_BRBNT=additions(df_BRBNT)
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=="c":#circular LeastSquareMethod
print("Curve fitting")
#3点ずつFITTINGして移動計算してradius centerY centerX　列を作成
#SKIONデータのCCWターンだけ抽出して右スキー谷足ターン弧のみ計算
#df_circle=df_BRBNT["iTOW_B0","diffzero","headMotMA3","roll","relPosN","Heading360","gSpeedB","pitch"]
#-------------relPをターン毎の座標に変換して新たな列を追加-----------------------------------
list_turnN=[]
list_turnNr=[]
list_turnNn=[]
list_turnD=[]
tn=0#ターンNo（headMotの極値のカウント）
tnr=0#ターンNoの行番号
tnr_1=0#ターンNoの一個前の行番号
for i in range(1,len(df_BRBNT)):
if df_BRBNT.loc[i,"diffzero"]>0:
tnr_1=tnr
tnr=i
tn=df_BRBNT.loc[i,"diffzero"]
list_turnN.append(tn)
list_turnNr.append(i)
df_BRBNT.loc[i,"turnLen"]=tnr
else:
df_BRBNT.loc[i,"turnLen"]=i-tnr
df_BRBNT.loc[i,"turnNo"]=tn
if df_BRBNT.loc[i,"headMotMA3"]-df_BRBNT.loc[i-1,"headMotMA3"]<0:
list_turnD.append("CCW")
df_BRBNT.loc[i,"turnD"]="CCW"
else:
list_turnD.append("CW")
df_BRBNT.loc[i,"turnD"]="CW"
df_BRBNT["turnNn"]=0
for i in range(0,len(list_turnN)-1): #turnの長さをカウント
list_turnNn.append(list_turnNr[i+1]-list_turnNr[i] )
df_BRBNT.loc[list_turnNr[i],"turnNn"]=list_turnNn[i]
print(i,list_turnN[i],list_turnNr[i],list_turnNn[i])
#-------------------relPosNEDをHPSたして作成-----------------------------------
df_BRBNT["relPosNh"]=0
df_BRBNT["relPosNh"]=df_BRBNT["relPosN_B"]#+df_BRBNT["relPosHPN_B"]/100)
#print("relPosNh=",df_BRBNT["relPosNh"])
df_BRBNT["relPosEh"]=0
df_BRBNT["relPosEh"]=df_BRBNT["relPosE_B"]#+df_BRBNT["relPosHPE_B"]/100)
#print("relPosEh=",df_BRBNT["relPosEh"])
df_BRBNT["relPosDh"]=0
df_BRBNT["relPosDh"]=df_BRBNT["relPosD_B"]#+df_BRBNT["relPosHPD_B"]/100)
#--------------------ターン毎にゼロ座標----------------------------------
df_BRBNT["relPosNt"]=0
df_BRBNT["relPosEt"]=0
df_BRBNT["relPosDt"]=0
for i in range(0,len(df_BRBNT)):
if df_BRBNT.loc[i,"turnNn"]>0:
turnnum=df_BRBNT.loc[i,"turnNn"]
kijunN=df_BRBNT.loc[i,"relPosNh"]
kijunE=df_BRBNT.loc[i,"relPosEh"]
kijunD=df_BRBNT.loc[i,"relPosDh"]
for j in range(1,turnnum):#i位置基準で座標作成
df_BRBNT.loc[i+j,"relPosNt"]=df_BRBNT.loc[i+j,"relPosNh"]-kijunN
df_BRBNT.loc[i+j,"relPosEt"]=df_BRBNT.loc[i+j,"relPosEh"]-kijunE
df_BRBNT.loc[i+j,"relPosDt"]=df_BRBNT.loc[i+j,"relPosDh"]-kijunD
df_BRBNT.to_csv(r'C:/RTK_LOG/_'+"_"+ basename, index=True) # CSV sav
print("Turn Zahyou FINISHED")
#-----------------円座標切り取ってFITTING------------------------------------
#３個ずつでFITTINGすれば最小円半径が得られるから真円なら同じ半径が続く
list_cxe=[]
list_cye=[]
list_re=[]
df_BRBNT["cxe"]=0
df_BRBNT["cye"]=0
df_BRBNT["re"]=0
xs=[]
ys=[]
df_BRBNT["cangle"]=0
df_BRBNT["sumdrr"]=0
df_BRBNT["sumskid"]=0
df_BRBNT["averoll"]=0
df_BRBNT["sum_deceler"]=0
df_BRBNT["dist"]=0
df_BRBNT["thetap"]=0
df_BRBNT["rad"]=0
for i in range(0,len(list_turnN)-1):
turnlen=list_turnNn[i]
step=turnlen-2
j=0
turnNo=df_BRBNT.loc[i,"turnNo"]
strow=list_turnNr[i]
endrow=strow+turnlen
xs=df_BRBNT.loc[strow:endrow,"relPosEt"]
ys=df_BRBNT.loc[strow:endrow,"relPosNt"]
print(i,":xs,ys=",xs,ys)
#円計算
x=np.array(xs)
y=np.array(ys)
(cxe,cye,re)=CircleFitting(x,y)
#----------------円最小二乗フィッティング　np.linalg.lstsq--------------
#A = np.vstack((x, y, np.ones((len(x))))).T
#v = -(x ** 2 + y ** 2)
#u, residuals, rank, s = np.linalg.lstsq(A, v, rcond=None)
list_cxe.append(cxe)
list_cye.append(cye)
list_re.append(re)
print("turnNo=",turnNo,strow,endrow,cxe,cye,re)
df_BRBNT.loc[strow,"cxe"]=cxe
df_BRBNT.loc[strow,"cye"]=cye
df_BRBNT.loc[strow,"re"]=re
#print(i,"df_BRBNT[cxe,cye,re]:",df_BRBNT.loc[strow,"cxe"],df_BRBNT.loc[strow,"cye"],df_BRBNT.loc[strow,"re"])
#------------ターン毎plot--------------------------------------------
#回帰円の座標を実データ座標xs,ysから角度を求めてxl,yl算出する
xe=[]#回帰円座標ｘ
ye=[]#回帰円座標y
#graph 初期
fig = plt.figure()
ax1 = fig.add_subplot()
#ax2=ax1.twinx()
print(df_BRBNT.loc[strow,"turnD"])
if turnlen>10 and df_BRBNT.loc[strow,"turnD"]=="CCW":
sumdrr=0
sumskid=0
sumroll=0
averoll=0
sumdeceler=0
for j in range(0,turnlen):#ターン内のデータ行毎で計算
cangle=math.atan2((y[j]-cye),(x[j]-cxe))
print("y[",j,"]=",y[j],"x[",j,"]=",x[j],"cangle=",cangle)
df_BRBNT.loc[strow+j,"cangle"]=cangle*57.29578
print(strow+j,"cangle=",df_BRBNT.loc[strow+j,"cangle"])
xv=re*math.cos(cangle)+cxe
yv=re*math.sin(cangle)+cye
xe.append(xv)
ye.append(yv)
#残差
L=cmath.sqrt((xv-cxe)*(xv-cxe)+(yv-cye)*(yv-cye))
dr=L-re
drr=dr*dr
sumdrr=sumdrr+drr
sumskid=sumskid+df_BRBNT.loc[j+strow,"skid"]
sumroll=sumroll+df_BRBNT.loc[j+strow,"roll"]
if df_BRBNT.loc[strow+j,"gSpeed_B"]-df_BRBNT.loc[strow+j-1,"gSpeed_B"]<0:
sumdeceler=sumdeceler+ df_BRBNT.loc[strow+j,"gSpeed_B"]-df_BRBNT.loc[strow+j-1,"gSpeed_B"]
print("trunNo=",i,"x[",j,"]=",x[j],"y[",j,"]=",y[j],"cangle,xe,ye",cangle,xv,yv,"sumdrr=",sumdrr)
df_BRBNT.loc[strow,"sumdrr"]=sumdrr.real
df_BRBNT.loc[strow,"sumskid"]=sumskid
averoll=sumroll/turnlen
df_BRBNT.loc[strow,"averoll"]=averoll
df_BRBNT.loc[strow,"sum_deceler"]=sumdeceler
#--------------Turn目標X座標データ作成-----------------
#目標点までの距離td、目標点までの角度ta
#目標点計算(tx,ty)
print("cangle:",df_BRBNT.loc[strow:endrow,"cangle"])
stangl=df_BRBNT.loc[strow,"cangle"] #atan2角度をGNSS角度変換
if stangl>90:
stgns=180-stangl+270
elif stangl>=0 and stangl<=90:
stgns=90-stangl
elif stangl<0:
stgns=90-stangl
endangl=df_BRBNT.loc[endrow-1,"cangle"]#atan2角度をGNSS角度変換
if endangl>90:
endgns=180-endangl+270
elif endangl>=0 and endangl<=90:
endgns=90-endangl
elif endangl<0:
endgns=90-endangl
theta=stgns-endgns#夾角
print("stangl,endangle=",stangl,endangl,"stgns,endgns=",stgns,endgns)
thetat=stgns-theta/2
print("theta=",theta,"thetat=",thetat,"cos=",math.cos(thetat/2*0.01745329),"sin=",math.sin(thetat/2*0.01745329))
#ターゲット角thetatの象限で計算角thetacがかわる
if thetat>270:
thetac=thetat-270
tx=cxe-re*math.cos(thetac*0.01745329)
ty=cye+re*math.sin(thetac*0.01745329)
elif thetat<270 and thetat>180:
thetac=270-thetat
tx=cxe-re*math.cos(thetac*0.01745329)
ty=cye-re*math.sin(thetac*0.01745329)
elif thetat<=180 and theta>=90:
thetac=thetat-180
tx=cxe+re*math.cos(thetac*0.01745329)
ty=cye-re*math.sin(thetac*0.01745329)
print("thetac=",thetac,"tx,ty=",tx,ty,",cos=",math.cos(thetac*0.01745329),",sin=",math.sin(thetac*0.01745329))
xp0=df_BRBNT.loc[strow,"relPosEt"]
yp0=df_BRBNT.loc[strow,"relPosNt"]
dist0=math.sqrt((xp0-tx)*(xp0-tx)+(yp0-ty)*(yp0-ty))
for j in range(1,turnlen):#ターン内のデータ行毎で計算
xp=df_BRBNT.loc[j+strow,"relPosEt"]
yp=df_BRBNT.loc[j+strow,"relPosNt"]
dist=math.sqrt((xp-tx)*(xp-tx)+(yp-ty)*(yp-ty))#ターゲット点と現在位置の距離接近はマイナス離れるとプラス
rad=math.sqrt((xp-cxe)*(xp-cxe)+(yp-cye)*(yp-cye))#現在位置と回転中心との距離　回転半径
thetap=math.atan2(yp-cye,xp-cxe)*57.29578
if thetap>90:#thetap中心からの角度をGNSS角度に変換
thetap=180-thetap+270
elif thetap<0 and thetap>-180:
thetap=90-thetap
elif thetap<=90 and thetap>=0:
thetap=90-thetap
print("thetat=",thetat,"thetap=",thetap)
df_BRBNT.loc[strow+j,"thetap"]=thetap
if thetap>=thetat:
dist=-dist
df_BRBNT.loc[strow+j,"dist"]=dist
df_BRBNT.loc[strow+j,"rad"]=rad
print(i,j,"dist=",dist,"thetap=",thetap,"rad=",rad)
#-----------------plot---------------------------
xep=np.array(xe)
yep=np.array(ye)
ax1.plot(x,y,color="blue",label="Ski trace")
ax1.plot(xep,yep,color="red",label="CircleLSM")
title=basename+"turnNo="+str(i)
fig.suptitle(title, y=0.995,size=8, weight=2, color="red")
txt1="Radius="+'{:.2f}'.format(re)+":Volkle RTM175 R=14m"
txt2="Cxe="+'{:.2f}'.format(cxe)+",Cye="+'{:.2f}'.format(cye)
txt3="Residual="+'{:.5e}'.format(sumdrr.real)
txt4="SumSkid="+'{:.2f}'.format(sumskid)
cosroll=math.cos(averoll*0.017453292)
txt5="AveRoll="+'{:.2f}'.format(averoll)+"COS(roll)="+'{:.2f}'.format(cosroll)
txt6="SumDeceleration="+'{:.2f}'.format(sumdeceler)
txt7="TargetPoint=("+'{:.2f}'.format(tx)+","+'{:.2f}'.format(ty)+")"
ax1.text(0.4, 0.9,txt1, fontfamily="serif", fontsize=10,transform=ax1.transAxes)
ax1.text(0.4, 0.85,txt2, fontfamily="serif", fontsize=10,transform=ax1.transAxes)
ax1.text(0.4, 0.8,txt3, fontfamily="serif", fontsize=10,transform=ax1.transAxes)
ax1.text(0.4, 0.75,txt4, fontfamily="serif", fontsize=10,transform=ax1.transAxes)
ax1.text(0.4, 0.70,txt5, fontfamily="serif", fontsize=10,transform=ax1.transAxes)
ax1.text(0.4, 0.65,txt6, fontfamily="serif", fontsize=10,transform=ax1.transAxes)
ax1.text(0.4, 0.60,txt7, fontfamily="serif", fontsize=10,transform=ax1.transAxes)
plt.legend(loc="lower left")
ax1.plot(tx,ty,"*",markersize=20)
fig.savefig(r'C:/RTK_Log/plot_'+str(i))
plt.show()
print("list_cxe:",list_cxe)
print("list_cye:",list_cye)
print("list_re:",list_re)
df_BRBNT.to_csv(r'C:/RTK_Log/CircleLSM_'+"_"+ basename, index=True) # CSV sav
elif v=="rc":#pph_sITPまとめファイルからsccwITPBNOファイルとsccwrollITPBNOファイル群を生成する
print("sITPまとめファイルを読み込む")
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 = pd.read_csv( filename, low_memory=True)
print(df.shape)
df["rolldiff"]=df["roll"].diff()
#hMmax=peaksHokancol(df,"headMot360")
df_peaks=peak_Maxmin(df,"headMot360")#headMotの山谷ピーク検出
df_rollpeaks=peak_Maxmin(df,"roll")#rollの山谷ピーク
print("df_rollpeaks=",df_rollpeaks)
#print("df_peaks=",df_peaks)
#=================headMotCCW　抽出=============================================
df_CCW=pd.DataFrame()
df_CCWS=pd.DataFrame()
coln=df.columns
#print("coln=",coln)
for i in range(0,len(df_peaks)):
ns=df_peaks.loc[i,"maxidx"]#maxidxからスタートして
ne=df_peaks.loc[i,"minidx"]#minidxまで切り取り　CCWのみ抽出
#print("df[ns:ne]=",ns,ne,df.loc[ns:ne,coln])
df_CCW=df.loc[ns:ne,coln]
#roll補正をいれてみる HY=10:roll=-30 HY=0:roll=-20 HY=
df_CCW["n0"]=df_peaks.loc[i,"n0"]
df_CCW["n1"]=df_peaks.loc[i,"n1"]
df_CCW["n2"]=df_peaks.loc[i,"n2"]
df_CCWS = pd.concat([df_CCWS, df_CCW],axis=0)
#print(i,":df_CCW=",df_CCW)
df_CCW.to_csv(r'C:/RTK_Log/hokan/sccwITPBNO_'+str(i)+'_'+ basenameITP, index=True) # CSV sav
df_CCWS.to_csv(r'C:/RTK_Log/hokan/sccwITPBNO_Allrc_'+ basenameITP, index=True) # CSV sav
#==============roll peak　ターン後半　山回り切り抜き=============================================
df_rCCW=pd.DataFrame()
df_rCCWS=pd.DataFrame()
rcoln=df.columns
#print("coln=",coln)
for i in range(0,len(df_rollpeaks)):
rns=df_rollpeaks.loc[i,"maxidx"]#maxidxからスタートして
rne=df_rollpeaks.loc[i,"minidx"]#minidxまで切り取り　CCWのみ抽出
rnend=df_rollpeaks.loc[i,"n2"]#谷から山の最後index
#print("df[ns:ne]=",ns,ne,df.loc[ns:ne,coln])
df_rCCW=df.loc[rne:rnend,coln]#rollの谷から山まで範囲を全体で切り取り
df_rCCW["n0"]=df_rollpeaks.loc[i,"n0"]
df_rCCW["n1"]=df_rollpeaks.loc[i,"n1"]
df_rCCW["n2"]=df_rollpeaks.loc[i,"n2"]
df_rCCWS = pd.concat([df_rCCWS, df_rCCW],axis=0)
#print(i,":df_CCW=",df_CCW)
df_rCCW.to_csv(r'C:/RTK_Log/hokan/sccwrollITPBNO_'+str(i)+'_'+ basenameITP, index=True) # CSV sav
df_rCCWS.to_csv(r'C:/RTK_Log/hokan/sccwrollITPBNO_Allrc_'+ basenameITP, index=True) # CSV sav
#===================PLOT=============================================
hMaxVal=df.loc[df_peaks["maxidx"],"headMot360"]
hMinVal=df.loc[df_peaks["minidx"],"headMot360"]
print("hMmaxVal=",hMaxVal)
# full data range plot
ns=0
ne=len(df)
hMfull=df.loc[ns:None,"headMot360"]
rollfull=df.loc[ns:None,"roll"]
iTOWfull=df.loc[ns:ne,"iTOW_B"]
x_full=np.arange(len(df))
x_Maxpeaks=df_peaks["maxidx"].to_numpy()
x_Minpeaks=df_peaks["minidx"].to_numpy()
y_full=hMfull.to_numpy()
yr_full=rollfull.to_numpy()
y_Maxpeaks=hMaxVal.to_numpy()
y_Minpeaks=hMinVal.to_numpy()
print("y_Maxpeaks=",y_Maxpeaks)
#x_full=iTOWfull.to_numpy()
fig = plt.figure()
# add_subplot()でグラフを描画する領域を追加する．引数は行，列，場所
ax1 = fig.add_subplot(1,1, 1,title="headMot360")
#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(x_Maxpeaks,y_Maxpeaks, color="red", marker = 'o',linewidth=0,label="headMot360 P+")
ax1.plot(x_Minpeaks,y_Minpeaks, color="green", marker = 'o',linewidth=0,label="headMot360 P-")
ax1.plot(x_full, y_full)
ax1.plot(x_full, yr_full)
plt.show()
#指定範囲プロット
#plt.scatter(x_observed, y_observed)
#plt.grid()
#plt.show()
#角速度で外れ値チェック
#df_new=angularCheck(df,"headMot_B")
#df_new=angularCheck(df_new,"Heading360")
print()
elif v=="auto":#H-Ysaの自己相関計算
print("Hy-sa　ファイル読み込み")
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 = pd.read_csv( filename, low_memory=True)
print(df.shape)
x=df["H-Ysag"].to_numpy()
corre = np.correlate(x, x, "full")
y=corre[int(corre.size/2):] / np.arange(len(x), 0, -1)
print("len(y)=",len(y))
X=np.arange(len(y))
Y=np.array(y)
fig = plt.figure()
ax1 = fig.add_subplot(1,1, 1)
ax1.plot(X,Y)
plt.show()
print()
elif v=='all':#>>>>sITPファイル群を指定して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)
elif v=='hoT' :#RTK全体の補間と補間後のdiffN計算追加 diffN毎にsITPBNO分割ファイル群を生成
print(" BRBNTファイル or BRBNファイルを読み込み済みのこと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
if BRBN_flag==0:
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))
#elif BRBN_flag==1:#BRBNファイルのみでBoots　Tファイルがない場合
#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"
#,"headMot360","Heading360","H-Ysa360"]
#print("list_call init len =",len(list_call))
elif BRBN_flag==1:#BRBNファイルのみでBoots　Tファイルがない場合
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"
,"headMot360"]
print("list_call init len =",len(list_call))
#<<<<BNO生データファイル読み込み-----------------------------------------------------
print(">>>> SELECT SkiOn_BNO FILE UBX readFILE=",ReadFile)
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)
"""
print(">>>> SELECT BootsOn_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)
basenameBNOT = os.path.basename(filename)
df_BNOfullT = pd.read_csv(filename, low_memory=True)
df_BNOfullT.columns=["bnitow","yaw","pitch","roll","ax","ay","az"]#コラムヘッダ名追加
print("df_BNOfullT:",df_BNOfull.shape,basenameBNO)
print(df_BNOfullT)
"""
#<<<------------------------------------------------------------------------------
klimit=len(diffN)-3
df_PDMdesAll=pd.DataFrame()
df_BRBNT1=pd.DataFrame()# 外れ値除去df
print("df_BRBNT=",df_BRBNT)
print("diffN=",diffN)
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の各コラムの外れ値除去　動きのデータの角速度で除外
#補間パラメータレンジ
dRange=df_BRBNT.loc[n0:n1,colname]#補間データの範囲List
iTOWB0Range=df_BRBNT.loc[n0:n1,"iTOW_B0"]#ゼロカウンタの範囲List
iTOWBRange=df_BRBNT.loc[n0:n1,"iTOW_B"]#iTOW_Bの範囲List
iTOWB_start=int(df_BRBNT.loc[n0,"iTOW_B"])#iTOW_B開始値
x_observed=iTOWB0Range.to_numpy()#X軸はiTOW_Bのゼロカウンタの範囲Listをdarry
x_iTOW_B=iTOWBRange.to_numpy()#X軸は、iTOW_B
y_observed=dRange.to_numpy()#Y軸は、dRangeをYデータとしてdarry
iTOW_B_init=df_BRBNT.loc[0,"iTOW_B"]
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)#X_latentでのY値をfit_result
list_iTOW_B=iTOWB_kizami.tolist()
list_interP = fit_result.tolist()
#-----------PLOT Colname-------------------------
if colname=="H-Ysa360":
fig = plt.figure()
# add_subplot()でグラフを描画する領域を追加する．引数は行，列，場所
ax1 = fig.add_subplot(1,1, 1,title=colname+"_sITP")
#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")
ax1.plot(x_iTOW_B,y_observed, color="red", marker = 'o',linewidth=0,label=colname+"_BRBNT")
#ax2.plot(x_observed,y_observed, color="red", label=colname+"_BRBNT")
#ax3.plot(x_observed,df_BRBNT.loc[n0:n1,colname], color="red")
#plt.scatter(iTOWB_kizami,fit_result)
#plt.scatter(x_observed,y_observed)
plt.title(colname+"-"+str(kaisu))
plt.show()
fig.savefig("C:/RTK_LOG/hokan/PLOT_H-Ysa360_"+str(kaisu)+"_.png")
print(str(kaisu)+":H-Ysa360:fit_result=",fit_result)
print(str(kaisu)+"H-Ysa360:y_observed=",y_observed)
print("list_interP=",list_interP)
#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)
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
kaisu+=3#kasiuを３個飛びで４STEP
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("<<<<<<<<<<<<<<<READ FILE=",filename,">>>>>>>>>>>>>>>>>>>>>>")
#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
elif v=='filt':
print("low pass filter ")
print(">>>> SELECT sITP_ _ FILE ")
#headMOtのピークを真の値としてHeading360とyawのピーク高さを合わせることで補正とする
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)
df = pd.read_csv(filename, low_memory=True)
#df_BNOfull.columns=["bnitow","yaw","pitch","roll","ax","ay","az"]#コラムヘッダ名追加
fn=filename
#print('[scipy.signal.find_peaks(x, height=None, threshold=None, distance=None,prominence=None, width=None, wlen=None, rel_height=0.5, plateau_size=None)]')
#fn='C:/RTK_Log/hokan/sITPBNO_51_True_BRBNTver042_03-15-11-39-44.binShorten_BTBNO.csv'
basenameITP = os.path.basename(fn)
#colist_org=["iTOW_B","Heading360","headMot360","headMotdiff","yaw","roll","H-Ysa360","skid","skidyaw"]
#listN_org=['0','3','6','9','12','15','18','21','24','27']#2024-06-14-09-43
print("df len=",len(df))
#------------butter worse--------------------------
# パラメータ設定
sample_freq = 100
cutoff_freq = 1
filter_order = 4
sec = len(df)*0.01
#f0 = 10
#f1 = 100
# ----yawHpgをフィルタ
t = np.linspace(0, sec , int(sample_freq * sec))
y=df.loc[:,"yawHpg"].to_numpy()
sos = signal.butter(filter_order, cutoff_freq, 'lowpass', output='sos', fs=sample_freq)
yf = signal.sosfiltfilt(sos, y)
print(yf)
# ----Heading360をフィルタ
y0=df.loc[:,"Heading360"].to_numpy()
sos = signal.butter(filter_order, cutoff_freq, 'lowpass', output='sos', fs=sample_freq)
y0f = signal.sosfiltfilt(sos, y0)
print(y0f)
#HYsa
df["HeadingFilt"]=pd.DataFrame(y0f)
df["yawFilt"]=pd.DataFrame(yf)
#print("df[yawFilt]=",df["yawFilt"])
df["HY_filt"]=df["Heading360"]-df["yawFilt"]
df["HY0_filt"]=df["HeadingFilt"]-df["yawFilt"]
#max min range
old_max=df["H-Ysapg"].max()
old_min=df["H-Ysapg"].min()
fltd_max=df["HY0_filt"].max()
fltd_min=df["HY0_filt"].min()
yHYpg=df["H-Ysapg"].to_numpy()
yHY0=df["HY0_filt"].to_numpy()
yHYflt=df["HY_filt"].to_numpy()
#print("df[HY_filt]=",df["HY_filt"])
df.to_csv(r'C:/RTK_Log/hokan/Filtered_'+basenameITP+".csv", index=True) # CSV sav
fig = plt.figure()
fig.suptitle("LowPassFiltered_"+str(cutoff_freq)+"Hz_"+basenameITP)
ax1 = fig.add_subplot(2,1, 1)
ax1.set_title("Heading,yaw plot")
ax1.plot(t,yf,label="Filtered")
ax1.plot(t,y,label="Not Filtered")
ax1.plot(t,y0,label="Target data")
ax1.plot(t,y0f,label="Target Filtered data")
plt.legend()
ax2 = fig.add_subplot(2,1, 2)
ax2.set_title("Error=Heading-yaw plot")
txt1="old:max="+str(round(old_max,1))+",min="+str(round(old_min,1))
txt2="Filtered:max="+str(round(fltd_max,1))+",min="+str(round(fltd_min,1))
ax2.text(t[0],old_max, txt1, size=10,color="black")
ax2.text(t[0],old_max-1,txt2, size=10,color="green")
ax2.plot(t,yHYpg,label="source data",color="black")
ax2.plot(t,yHYflt,label="Filtered data",color="blue")
ax2.plot(t,yHY0,label="HeadingFiltered data",color="green")
plt.legend()
plt.show()
fig.savefig("C:/RTK_Log/hokan/PLOT_LowPassFiltered_"+str(cutoff_freq)+"Hz_"+basenameITP+".png")
elif v=='lpf':#既存のファイル群をまとめてフィルター処理を追加
print("low pass filter ")
print(">>>> SELECT sITP_ _ FILE ")
#headMOtのピークを真の値としてHeading360とyawのピーク高さを合わせることで補正とする
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)
#シリーズファイル群まとめて処理
fsname=csv_Allname(filename)#ファイルシリーズのフル名のソート済みリスト
for i in range (0,len(fsname)):
print(i,":",fsname[i])
#==============LPG処理======================================
i=0
list_stat=[]
list_statAll=[]
while i<len(fsname):
print("====>",i,fsname[i])
fn=fsname[i]
basename = os.path.basename(fn)
df = pd.read_csv(fn, low_memory=True)
df=LPF(df,"Heading360")#LowpassFilter関数
df=LPF(df,"yawHpg")#LowpassFilter関数
print(df.columns)
print("df[yasHg_f]=",df["yawHpg_f"])
df["H-Ysapg_f"]=df["Heading360_f"]-df["yawHpg_f"]
df.to_csv(r"C:/RTK_Log/hokan/LPF/LPF_LOG_"+str(i)+"_"+basename+".csv", index=True) # CSV sav
#max min range
old_max=df["H-Ysapg"].max()
old_min=df["H-Ysapg"].min()
old_ave=df["H-Ysapg"].mean()
old_std=df["H-Ysapg"].std()
old_range=old_max-old_min
fltd_max=df["H-Ysapg_f"].max()
fltd_min=df["H-Ysapg_f"].min()
fltd_ave=df["H-Ysapg_f"].mean()
fltd_std=df["H-Ysapg_f"].std()
fltd_range=fltd_max-fltd_min
list_stat=[old_max,old_min,old_ave,old_std,old_range,fltd_max,fltd_min,fltd_ave,fltd_std,fltd_range]
list_statAll.append(list_stat)
#plot準備
fig = plt.figure()
x=np.arange(len(df))
yHD0=df["Heading360"].to_numpy()
yHDf=df["Heading360_f"].to_numpy()
yyw0=df["yawHpg"].to_numpy()
yywf=df["yawHpg_f"].to_numpy()
yHY0=df["H-Ysapg"].to_numpy()
yHYf=df["H-Ysapg_f"].to_numpy()
#plot
ax1 = fig.add_subplot(2,1, 1)
fig.suptitle("LPF_1Hz_"+fsname[i])
ax1.set_title("Heading,yaw plot")
ax1.plot(x,yHD0,label="Heading360",color="orange")
ax1.plot(x,yHDf,label="Filtered Heading360 ",color="red")
ax1.plot(x,yyw0,label="yawHpg",color="blue")
ax1.plot(x,yywf,label="Filtered yawHpg",color="green")
plt.legend()
ax2 = fig.add_subplot(2,1, 2)
ax2.set_title("Error=Heading-yaw plot")
txt1="old:max="+str(round(old_max,1))+",min="+str(round(old_min,1))
txt2="Filtered:max="+str(round(fltd_max,1))+",min="+str(round(fltd_min,1))
ax2.text(x[0],old_max, txt1, size=10,color="black")
ax2.text(x[0],old_max-1,txt2, size=10,color="red")
ax2.plot(x,yHY0,label="source Error data",color="black")
ax2.plot(x,yHYf,label="Filtered Error data",color="red")
#ax2.plot(t,yHY0,label="HeadingFiltered data",color="green")
plt.legend()
plt.show()
fig.savefig("C:/RTK_Log/hokan/LPF/PLOT_LPF1Hz_"+basename+".png")
i+=1
df_stat=pd.DataFrame(list_statAll)
df_stat.columns=["old_max","old_min","old_ave","old_std","old_range","fltd_max","fltd_min","fltd_ave","fltd_std","fltd_range"]
df_stat.to_csv(r"C:/RTK_Log/hokan/LPF/LPF_STAT_0-"+str(i)+"_"+basename+".csv", index=True) # CSV sav
print()
# 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=='jp':#jointplot
print(">>>> SELECT sITPBNO 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 = pd.read_csv(filename, low_memory=True)
#x=df["H-Ysag"].to_numpy()
#y=df["roll"].to_numpy()
sns.jointplot(y='H-Ysag', x='roll', data=df,kind="reg")
plt.show()
#g.savefig('C:/RTK_Log/hokan/jointP_'+basenameITPRTK+'.png')
elif v=='regs' :#pp済みファイルのroll回帰専用 sccwroll　まとめて出力
print("Regression 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)
#連番ファイルをディレクトリーから検索ソートしてlist_fsnameに渡す
lsit_fsname=[]
list_fsname=csv_Allname(filename)
print("len(linst_fsname)=",len(list_fsname))
# 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"]]
i=0
list_cor=[]
while i<len(list_fsname): #連番ファイルセットで順次読んで処理
df= pd.read_csv(list_fsname[i], low_memory=True)
print(i,":read list_fsname=",list_fsname[i])
df=df.loc[:,["iTOW_B","headMot360","Heading360","roll","yawHg","H-Ysag","yawHpg"]]
#=========numy polyfitで回帰式計算 colnで与えられたカラムを総当たりで計算する=========
coln=["iTOW_B","headMot360","Heading360","roll","yawHg","H-Ysag","yawHpg"]
list_coln=list(range(0,len(coln)-1,1))
#print("list_coln=",list_coln)
#df統計値
rmax=df["roll"].max()
rmin=df["roll"].min()
HYmax=df["H-Ysag"].max()
HYmin=df["H-Ysag"].min()
#print("rmax=",rmax,"rmin=",rmin,"HYmax=",HYmax,"HYmin=",HYmin)
#roll 時系列直線近似
#print("df.columns=",df.columns)
x=np.arange(len(df))#行番号をX軸
y=df.loc[:,"roll"].to_numpy()
zr = np.polyfit(x, y, 1)
cr=np.corrcoef(x,y)
rslope=zr[0]
rintc=zr[1]
rcorr=cr[0][1]
#print("reg:rslope=",rslope,"rintc=",rintc,"rcorr=",rcorr)
#roll-HYsag相関計算
x=df.loc[:,"roll"].to_numpy()
y=df.loc[:,"H-Ysag"].to_numpy()
z = np.polyfit(x, y, 1)
c=np.corrcoef(x,y)
#print("slope=",z[0],"intercept=",z[1],"correl=",c[0][1])
list_cor.append([i,z[0],z[1],c[0][1],rmax,rmin,HYmax,HYmin,rslope,rintc,rcorr,list_fsname[i]])
i+=1
corcol=["No","slope","intercept","correl","rmax","rmin","HYmax","HYmin","rslope","rintc","rcorr","fsname"]
df_cor=pd.DataFrame(list_cor)
df_cor.columns=corcol
print("df_cor=",df_cor)
df_cor.to_csv(r'C:/RTK_Log/hokan/regAllCor_'+basenameITPRTK+".csv", index=True) # CSV sav
print("SAVED:C:/RTK_Log/hokan/regAllCor_"+basenameITPRTK+".csv")
elif v=='fft':
print("FFT")
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 = 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"]]
#df_pplot=df_ITPRTK.loc[:,["iTOW_B","headMot360","Heading360","roll","yawHg","H-Ysag","yawHpg","ax","ay","az"]]
#df=df_pplot
#fft plot------------------
dfx=df.loc[:,["ax"]]
F = np.fft.fft(dfx,dfx)
Ampx = np.abs(F)
print(Ampx)
dfy=df.loc[:,["ay"]]
F = np.fft.fft(dfy)
Ampy = np.abs(F)
print(Ampy)
dfz=df.loc[:,["az"]]
F = np.fft.fft(dfz)
Ampz = np.abs(F)
print(Ampz)
#time plot-----------------
tY0=df.loc[:,"ax"].to_numpy()
tY1=df.loc[:,"ay"].to_numpy()
tY2=df.loc[:,"az"].to_numpy()
fig = plt.figure()
ax1 = fig.add_subplot(2,1,1)
ax2= fig.add_subplot(2,1,2)
x=np.arange(len(df))#行番号をX軸
y0=Ampx
y1=Ampy
y2=Ampz
ax1.plot(x,y0)
ax1.plot(x,y1)
ax1.plot(x,y2)
ax2.plot(x,tY0)
ax2.plot(x,tY1)
ax2.plot(x,tY2)
plt.show()
print()
elif v=='pp' :#pph_sITPBNOファイル用ペアプロット
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"]]
df_pplot=df_ITPRTK.loc[:,["iTOW_B","headMot360","Heading360","roll","yawHg","H-Ysag","yawHpg","rolldiff"]]
df=df_pplot
#=========numy polyfitで回帰式計算 colnで与えられたカラムを総当たりで計算する=========
coln=["iTOW_B","headMot360","Heading360","roll","yawHg","H-Ysag","yawHpg","rolldiff"]
list_coln=list(range(0,len(coln)-1,1))
print("list_coln=",list_coln)
list_cor=[]
i=0
for pair in itertools.combinations(list_coln, 2):
n0=int(pair[0])
n1=int(pair[1])
Xcol=df.columns[n0]
Ycol=df.columns[n1]
print(n0,":",Xcol,n1,":",Ycol)
x=df.iloc[:,n0].to_numpy()
y=df.iloc[:,n1].to_numpy()
z = np.polyfit(x, y, 1)
c=np.corrcoef(x,y)
print("slope=",z[0],"intercept=",z[1],"correl=",c[0][1])
list_cor.append([i,Xcol,Ycol,z[0],z[1],c[0][1]])
i+=1
corcol=["No","Xcol","Ycol","slope","intercept","correl"]
df_cor=pd.DataFrame(list_cor)
df_cor.columns=corcol
print("df_cor=",df_cor)
df_cor.to_csv(r'C:/RTK_Log/hokan/ppCor_'+basenameITPRTK+".csv", index=True) # CSV sav
#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)
supname=basenameITPRTK
pairplot(df_pplot,supname)#相関付きペアプロット
#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()
elif v=='ppreg' :#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"]]
#df_pplot=df_ITPRTK.loc[:,["iTOW_B","headMot360","Heading360","roll","yawHg","H-Ysag","yawHpg"]]
df_pplot=df_ITPRTK.loc[:,["No","slope","intercept","correl","rmax","rmin","HYmax","HYmin","rslope","rintc","rcorr"]]
df=df_pplot
#=========numy polyfitで回帰式計算 colnで与えられたカラムを総当たりで計算する=========
coln=["iTOW_B","headMot360","Heading360","roll","yawHg","H-Ysag","yawHpg"]
list_coln=list(range(0,len(coln)-1,1))
print("list_coln=",list_coln)
list_cor=[]
i=0
for pair in itertools.combinations(list_coln, 2):
n0=int(pair[0])
n1=int(pair[1])
Xcol=df.columns[n0]
Ycol=df.columns[n1]
print(n0,":",Xcol,n1,":",Ycol)
x=df.iloc[:,n0].to_numpy()
y=df.iloc[:,n1].to_numpy()
z = np.polyfit(x, y, 1)
c=np.corrcoef(x,y)
print("slope=",z[0],"intercept=",z[1],"correl=",c[0][1])
list_cor.append([i,Xcol,Ycol,z[0],z[1],c[0][1]])
i+=1
corcol=["No","Xcol","Ycol","slope","intercept","correl"]
df_cor=pd.DataFrame(list_cor)
df_cor.columns=corcol
print("df_cor=",df_cor)
df_cor.to_csv(r'C:/RTK_Log/hokan/ppCor_'+basenameITPRTK+".csv", index=True) # CSV sav
#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)
supname=basenameITPRTK
pairplot(df_pplot,supname)#相関付きペアプロット
#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()
elif v=='ppc' :#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"]]
df_pplot=df_ITPRTK.loc[:,["iTOW_B","headMot360","Heading360","roll","yawHg","H-Ysag","yawHpg"]]
df=df_pplot
#=========numy polyfitで回帰式計算 colnで与えられたカラムを総当たりで計算する=========
coln=["iTOW_B","headMot360","Heading360","roll","yawHg","H-Ysag","yawHpg"]
list_coln=list(range(0,len(coln)-1,1))
print("list_coln=",list_coln)
list_cor=[]
i=0
for pair in itertools.combinations(list_coln, 2):
n0=int(pair[0])
n1=int(pair[1])
Xcol=df.columns[n0]
Ycol=df.columns[n1]
print(n0,":",Xcol,n1,":",Ycol)
x=df.iloc[:,n0].to_numpy()
y=df.iloc[:,n1].to_numpy()
z = np.polyfit(x, y, 1)
c=np.corrcoef(x,y)
print("slope=",z[0],"intercept=",z[1],"correl=",c[0][1])
list_cor.append([i,Xcol,Ycol,z[0],z[1],c[0][1]])
i+=1
corcol=["No","Xcol","Ycol","slope","intercept","correl"]
df_cor=pd.DataFrame(list_cor)
df_cor.columns=corcol
print("df_cor=",df_cor)
df_cor.to_csv(r'C:/RTK_Log/hokan/ppCor_'+basenameITPRTK+".csv", index=True) # CSV sav
print("SAVED:C:/RTK_Log/hokan/ppCor_"+basenameITPRTK+".csv")
#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)
#supname=basenameITPRTK
#pairplot(df_pplot,supname)#相関付きペアプロット
#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)
print("filename=",filename)
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=='call':
print(">>>>>>>>>>>>>SELECT Group files ")
list_result=[]
root = tkinter.Tk()
root.withdraw()
iDir = r'C:/RTK_Log/'
fTyp = [("データファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
basenameITP = os.path.basename(filename)
df=csv_All_make(filename)
elif v=='pph' :#sITPファイル１個のピーク検出してピーク別ファイルpph_を1個生成　グラフプロット
#作成新規カラム"H-Ysag","yawHg","yawHpg"
print(">>>>>>>>>>>>>SELECT sITP files ")
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 = pd.read_csv( filename, low_memory=True)
print(df.shape)
#graph 初期
fig = plt.figure()
ax1 = fig.add_subplot(1,1,1)
ax2=ax1.twinx()
#yaw 180ジャンプ修正
df["yaw360"] = np.where(df["yaw"] < 0, df["yaw"] + 360, df["yaw"])
df["yaw"]=df["yaw360"]
#加工用計算---------------------------------------------------------
#平均
hM_ave=df.loc[500:1000,"headMot360"].mean()
Hd_ave=df.loc[500:1000,"Heading360"].mean()
yaw_ave=df.loc[500:1000,"yaw"].mean()
g=Hd_ave-yaw_ave
#g固定用基準ファイルを読んだ後に基準ファイルのg値をここに手入力する
g=55.5363
df["yawHg"]=df["yaw"]+g#初期補正済みyawHg gを足す
print("ave:hM,Hd,yaw,g=",hM_ave,Hd_ave,yaw_ave,g)
df["H-Ysag"]=df["Heading360"]-df["yawHg"]
print("H-Ysag=",df["H-Ysag"])
#ピーク
fpr=peaksHokancol(df,"headMot360")
fpr1=peaksHokancol(df,"yaw")#Find Peaks
fpr2=peaksHokancol(df,"Heading360")#Find Peaks
fcr=crossPoints(df,fpr1,fpr2)#Find Croaapoints
turnrange=[]
Lmin=[]
Lminidx=[]
HYstat=[]
HYsap=[]#Peak補正済みのyaw配列
#統計処理======================================================================
for i in range(0,len(fpr)):#左ターン部分の統計計算　範囲縦ラインデータturnrange
n0=fpr[i]#headMotの山ピーク位置
#n_1=fpr1[i]#yaw360の山ピーク位置
#n_2=fpr2[i]#Heading360の山ピーク位置
if i==len(fpr)-1:
n1=len(df)
else:
n1=fpr[i+1]#次のピークの先頭が最後
#yawの底ピーク位置を左ターンの最後にする
yawmin=df.loc[n0:n1,"yawHg"].min()
yawminidx=df.loc[n0:n1,"yawHg"].idxmin()
Lmin.append(yawmin)
Lminidx.append(yawminidx)
n01=Lminidx[i]#yaw360の谷ピーク位置
turnrange.append(n0)#
turnrange.append(yawminidx)
df["H-Ysapg"]=0
#===========ピーク補正生成 hpg gレベルをゼロとしてpeakとの差======================================
hpg=df.loc[fpr[i],"headMot360"]-df.loc[fpr[i],"yawHg"]
print("hpg=",hpg)
df.loc[n0:n01,"H-Ysapg"]=df.loc[n0:n01,"H-Ysag"]-hpg
print("df['H-Ysapg']=",df["H-Ysapg"])
print("df['H-Ysag']=",df["H-Ysag"])
df["yawHpg"]=df["yawHg"]+hpg#初期補正済みyawHg gを足す
#=============================================================
#統計量　roll
roll_ave=df.loc[n0:n01,"roll"].mean()
roll_max=df.loc[n0:n01,"roll"].max()
roll_min=df.loc[n0:n01,"roll"].min()
roll_std=df.loc[n0:n01,"roll"].std()
roll_rng=roll_max - roll_min
#統計計算 初期補正gレベル
HYsag_ave=df.loc[n0:n01,"H-Ysag"].mean()
HYsag_max=df.loc[n0:n01,"H-Ysag"].max()
HYsag_min=df.loc[n0:n01,"H-Ysag"].min()
HYsag_std=df.loc[n0:n01,"H-Ysag"].std()
HYsag_rng=HYsag_max-HYsag_min
#headMot　Peak補正レベル
HYsapg_ave=df.loc[n0:n01,"H-Ysapg"].mean()
HYsapg_max=df.loc[n0:n01,"H-Ysapg"].max()
HYsapg_min=df.loc[n0:n01,"H-Ysapg"].min()
HYsapg_std=df.loc[n0:n01,"H-Ysapg"].std()
HYsapg_rng=HYsapg_max-HYsapg_min
HYstat.append([HYsag_ave,HYsag_max,HYsag_min,HYsag_std,HYsag_rng,HYsapg_ave,HYsapg_max,HYsapg_min,HYsapg_std,HYsapg_rng,n0,n01,g,hpg,roll_ave,roll_max,roll_min,roll_std,roll_rng])
print(HYstat)
print("turnrange=",turnrange)
df_stat=pd.DataFrame(HYstat)
df_stat.columns=["HYsag_ave","HYsag_max","HYsag_min","HYsag_std","HYsag_rng","HYsapg_ave","HYsapg_max","HYsapg_min","HYsapg_std","HYsapg_rng","n0","n01","g","hpg","roll_ave","roll_max","roll_min","roll_std","roll_rng"]
#print("df_HYresult=",df_HYresult)
df_stat.to_csv(r'C:/RTK_Log/hokan/pph_Stat_'+basenameITP+ "_g=" +str(int(g))+".csv", index=True) # CSV sav
df.to_csv(r'C:/RTK_Log/hokan/pph_Log_'+basenameITP+ "_g=" +str(int(g))+".csv", index=True) # CSV sav
#====================================================================================
#-------------------------------------------------------------------
#ピーク点プロット
yhMp=df.loc[fpr,"headMot360"].to_numpy()
yyawp=df.loc[fpr1,"yaw"].to_numpy()
yHdp=df.loc[fpr2,"Heading360"].to_numpy()
yhMc=df.loc[fcr,"headMot360"].to_numpy()
ax1.plot(fpr,yhMp, color="red",marker = 'o',linewidth=0)#Peak headMot
ax1.plot(fcr,yhMc, color="green",marker = 'o', linewidth=0)#Cross Point headMot-Heading
ax1.plot(fpr1,yyawp, color="lime",marker = 'o', linewidth=0)#Peaks yaw
ax1.plot(fpr2,yHdp, color="blue",marker = 'o', linewidth=0)#Peaks Heading360
#時系列プロット
x0=np.arange(len(df))
yhM=df["headMot360"].to_numpy()
yHd=df["Heading360"].to_numpy()
yyaw=df["yaw"].to_numpy()
yyawHg=df["yawHg"].to_numpy()
yyawHpg=df["yawHpg"].to_numpy()
yroll=df["roll"].to_numpy()
yHyg=df["H-Ysag"].to_numpy()
yHypg=df["H-Ysapg"].to_numpy()
#xitB=df["iTOW_B"].to_numpy()
ax2.hlines(y=-5,xmin=0, xmax=len(df),color="black")
ax2.hlines(y=5,xmin=0, xmax=len(df),color="black")
ax1.vlines(turnrange,ymin=0,ymax=500,color="black")
# add_subplot()でグラフを描画する領域を追加する．引数は行，列，場所
ax1.plot(x0,yhM,color="blue",label="headMot360")
ax1.plot(x0,yHd,color="red",label="Heading360")
ax1.plot(x0,yyaw,color="green",label="yaw")
ax1.plot(x0,yyawHg,color="lime",label="yawHg")
ax1.plot(x0,yyawHpg,color="darkgreen",label="yawHpg")
ax1.plot(x0,yroll,color="black",label="roll")
ax2.plot(x0,yHyg,color="aqua",label="H-Ysag")
ax2.plot(x0,yHypg,color="darkgreen",label="H-Ysapg")
title=basenameITP
fig.suptitle(title, y=0.995,size=8, weight=2, color="red")
#plt.legend(loc="upper left")
ax1.legend(loc="upper left")
ax2.legend(loc="upper right")
plt.show()
txt=filename+"_g="+str(int(g))+"_pph.png"
fig.savefig(txt,dpi=600)
elif v=='fpT':#find_peaks df_test
print(">>>> SELECT sITP_ _ FILE ")
#headMOtのピークを真の値としてHeading360とyawのピーク高さを合わせることで補正とする
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)
df = pd.read_csv(filename, low_memory=True)
#df_BNOfull.columns=["bnitow","yaw","pitch","roll","ax","ay","az"]#コラムヘッダ名追加
fn=filename
#print('[scipy.signal.find_peaks(x, height=None, threshold=None, distance=None,prominence=None, width=None, wlen=None, rel_height=0.5, plateau_size=None)]')
#fn='C:/RTK_Log/hokan/sITPBNO_51_True_BRBNTver042_03-15-11-39-44.binShorten_BTBNO.csv'
basenameITP = os.path.basename(fn)
colist_org=["iTOW_B","Heading360","headMot360","headMotdiff","yaw","roll","H-Ysa360","skid","skidyaw"]
listN_org=['0','3','6','9','12','15','18','21','24','27']#2024-06-14-09-43
#listN_org=['30','33','36','39','42','45','48','51']#2024-06-16-18-27
#listN_org=['6','9']
listNstr= ','.join(listN_org)
df=sITP_dfMake(fn,listN_org,colist_org,"ng")
#-----yaw　yaw360変換------------------------------------------
#df["yaw360"] = np.where(df["yaw"] < 0, df["yaw"] + 360, df["yaw"])
df["yaw360"]=df["yaw"]
print("yaw360mean=",df["yaw360"].mean())
#---H-Ysa36t0N ---
df["H-YsaNew"]=df["Heading360"]-df["yaw360"]
print("H-Ysa=",df["H-YsaNew"]-df["H-Ysa360"])
#==========FIND PEAKS headMotのピークにyawをあわせるyawのピークは関与しない====================
fpr=peaksHokancol(df,"headMot360")
fpr1=peaksHokancol(df,"yaw360")#Find Peaks
fpr2=peaksHokancol(df,"Heading360")#Find Peaks
fcr=crossPoints(df,fpr1,fpr2)#Find Croaapoints
print("CrossPoint:fcr=",fcr)
print("Peaks headMot:fpr=",fpr)
print("Peaks yaw:fpr1=",fpr1)
print("Peaks Heading:fpr2=",fpr2)
y=df["headMot360"].to_numpy()
print("y=",y)
y1=df["yaw360"].to_numpy()
print("y1=",y1)
y2=df["Heading360"].to_numpy()
print("y2=",y2)
#fpr,_=find_peaks(y,height=150,distance=100,threshold=0.1)#RターンピークheadMot
print("fpr=",fpr,y[fpr])
#平均値でH-Ysaの下駄g
yaw360_ave=df.loc[:,"yaw360"].mean()
Heading360_ave=df.loc[:,"Heading360"].mean()
g= Heading360_ave-yaw360_ave#H-Ysa下駄
print("geta=",g,yaw360_ave,Heading360_ave)
#======== PEAKS fprデータから補正データ作成=============================
#----補正区間 fpr[n]-fpr[n+1]のmin値が底ピークとしてlpeak
#hd=heaMot-yaw
Lmin=[]#Rターン最後の底値
Lminidx=[]#Rターン最後の底値のindex番号
i=0
HYresult=[]#補正有無統計List
df["hd"]=0#headMot360とyawのピーク差
df["hh"]=0#headMot360とHeading360のピーク差
df["fpr0"]=0
if len(fpr)>len(fpr1)>=len(fpr2) or len(fpr1)>len(fpr)>=len(fpr2):
num=len(fpr2)
print("Least num: fpr2= ",num)
elif len(fpr)>len(fpr2)>=len(fpr1) or len(fpr2)>len(fpr)>=len(fpr1):
num=len(fpr1)
print("Least num: fpr1= ",num)
else:
num=len(fpr)
print("Least num: fpr= ",num)
#====================fpr LOOP================================
while i<num:#fpr 数で回す
#位置決め
n0=fpr[i]#headMotの山ピーク位置
n_1=fpr1[i]#yaw360の山ピーク位置
n_2=fpr2[i]#Heading360の山ピーク位置
if i==len(fpr)-1:
n1=len(df)
else:
n1=fpr[i+1]
#底ピーク位置
Lmin.append(df.loc[n0:n1,"yaw360"].min())
Lminidx.append(df.loc[n0:n1,"yaw360"].idxmin())
#print(i,"=>n0:n1 min=",n0,n1,Lmin[i],Lminidx[i])
n01=Lminidx[i]#yaw360の谷ピーク位置
#print("headMotYama_n0=",n0,"yawTani_n01=",n01,"yawYama_n_1=",n_1,"HeadinfYama_n_2=",n_2)
#yaw補正を区間ごとにつくる
#hd=y[fpr[0]]-y1[fpr1[0]]#最初の方法
#hd=y[fpr[i]]-df.loc[fpr[i],"yaw360"]#headMOtのピーク位置fpr[0]の角度y[fpr[0]]と同位置のyaw360
#**************************補正値計算*******************************
hd=y[fpr[i]]-df.loc[fpr1[i],"yaw360"]#yawピークとheadMotピークを合わせる補正方法
#hh=y[fpr[i]]-df.loc[fpr2[i],"Heading360"]#headMotのピークとHeading360ピークを合わせる補正方法
hc=df.loc[fcr[i],"headMot360"]-df.loc[fcr[i],"yaw360"]#headMot360とのHeading360交点とyaw高さを合わせる補正方法
print("**********i=",i,"****************")
print("hc=",hc,"fcr[",i,"]=",fcr[i])
print("df[fcr[i],'headMot']=",df.loc[fcr[i],"headMot360"])
print("df.loc[fcr[i],'yaw360']",df.loc[fcr[i],"yaw360"])
#*************************************************************
df.loc[n0:n1,"hd"]=hd
df.loc[n0:n1,"fpr"]=fpr[i]
df.loc[n_1:n1,"fpr1"]=fpr1[i]
df.loc[n_2:n1,"fpr2"]=fpr2[i]
df.loc[n_1:n01,"yawH"]=df.loc[n_1:n01,"yaw360"]+g
df.loc[n_1:n01,"yawHC"]=df.loc[n_1:n01,"yaw360"]+hc
#df.loc[n_2:n01,"HeadingH"]=df.loc[n_2:n01,"Heading360"]+hh
df.loc[n0:n01,"H-YsaN"]=df.loc[n0:n01,"H-YsaNew"]-g
df.loc[n0:n01,"H-YsaH"]=df.loc[n0:n01,"Heading360"]-df.loc[n0:n01,"yawH"]
df.loc[n0:n01,"H-YsaHC"]=df.loc[n0:n01,"Heading360"]-df.loc[n0:n01,"yawHC"]
df.loc[n01:n1,"H-YsaH"]=0