-
Notifications
You must be signed in to change notification settings - Fork 0
/
find_frequency.py
104 lines (84 loc) · 2.64 KB
/
find_frequency.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
import struct
import matplotlib.pyplot as plt
import numpy as np
import os
from scipy import signal
from math import pi
import pywt
import struct
plt.rc('font',family='Times New Roman',size=10)
markers_freq = ['.','o','v','1','2','3','4','^','<','>','s','p','*','h','H','+','x','D','d','|','_',',']
fig = plt.figure(figsize=(5,5))
sgn = 1.0
interval = 5
dt = 0.0000001 * interval
fs = 10000000 / interval
start = 250000
end = 350000
result = []
pwd = os.getcwd()
dict_fitting = {}
for dd in np.arange(40,160,10):
count = 0
record = []
while True:
filepath = pwd + "//" + "input_data" + "//" + "5" + "//" + str(dd) + "//" + str(count)
if os.path.isfile(filepath) == False:
break
with open(filepath, "rb") as fb:
data = fb.read()
ch1ch2 = struct.unpack("<"+str(int(len(data)/2))+"H", data)
ch1ch2 = np.array(ch1ch2)
ch1ch2 = (ch1ch2-8192)*2.5/8192
datay1 = ch1ch2[::2]
datay2 = ch1ch2[1::2]
data1 = datay1[start:end:interval]
data2 = datay2[start:end:interval]
wavelet = 'morl'
c = pywt.central_frequency(wavelet)
fa = np.arange(20000, 400000 + 1, 10000)
scales = np.array(float(c)) * fs / np.array(fa)
[cfs1,frequencies1] = pywt.cwt(data1,scales,wavelet,dt)
[cfs2,frequencies2] = pywt.cwt(data2,scales,wavelet,dt)
power1 = (abs(cfs1)) ** 2
power2 = (abs(cfs2)) ** 2
for i,f in enumerate(fa):
mean1 = power1[i].mean()
power1[i] = power1[i] / mean1
mean2 = power2[i].mean()
power2[i] = power2[i] / mean2
temp = signal.correlate(power1[i],power2[i], mode='same',method='fft')
corr = (np.where(temp == max(temp))[0][0]-len(temp) / 2 ) * dt * 1000
dict_fitting[f] = [dd, corr]
print(filepath)
count = count + 1
if count > 20:
break
plt.subplot(2,1,1)
x = []
y = []
for index,item in enumerate(result):
for yi in item:
xi = 120 - index * 20
x.append(xi)
y.append(yi)
plt.plot(xi, yi, '+')
f = np.polyfit(x,y,1)
xf = [120 - i * 20 for i in range(0,13)]
yf = [xi * f[0] + f[1] for xi in xf]
plt.plot(xf,yf,'r-')
plt.ylabel('time difference/ms')
print(f)
plt.subplot(2,1,2)
error = []
for index,item in enumerate(result):
for yi in item:
xi = 120 - index * 20
xf = (yi - f[1])/f[0]
error.append(abs(xf - xi))
plt.plot(xi,xf,'+')
plt.ylabel('calculation result/cm')
print(np.mean(error))
plt.xlabel('distance difference/cm')
plt.subplots_adjust(bottom = 0.2,left = 0.15,right=0.8)
plt.show()