223 lines
4.3 KiB
Python
223 lines
4.3 KiB
Python
import matplotlib
|
|
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
|
|
|
|
plt.rcParams['font.sans-serif']=['SimHei']
|
|
plt.rcParams['axes.unicode_minus']=False
|
|
|
|
|
|
x=np.arange(0,np.pi,0.0025)
|
|
y=np.sin(x)*0.5
|
|
|
|
|
|
# 添加方波
|
|
def add_line(x):
|
|
ret=[]
|
|
length=len(x)
|
|
num=length//40
|
|
index=0
|
|
t_old=0
|
|
for i in x:
|
|
t=(index//num)
|
|
if(t_old!=t):
|
|
h=np.random.random_sample()
|
|
t_old=t
|
|
if((t&1)!=0):
|
|
t=index%num
|
|
if(t<(num//2)):
|
|
ret.append(((0.15+h)/num)*(index%num))
|
|
else:
|
|
ret.append(((0.15+h)/num)*(num-index%num))
|
|
# ret.append(0.15+((h)))
|
|
else:
|
|
ret.append(0)
|
|
index+=1
|
|
return ret
|
|
y=add_line(x)+y
|
|
|
|
# 添加噪声
|
|
def add_random(x):
|
|
ret=[]
|
|
length=len(x)
|
|
index_table=[]
|
|
for i in range(20):
|
|
index_table.append(int(np.random.random_sample()*10000%length))
|
|
# print(index_table)
|
|
for i in range(length):
|
|
if(i in index_table):
|
|
ret.append(x[i]+np.random.random_sample()-0.5)
|
|
else:
|
|
ret.append(x[i])
|
|
return ret
|
|
|
|
|
|
# 卡尔曼
|
|
class kalman:
|
|
LastP=0.02 #上次估算的协方差
|
|
Now_P=0# 当前估算的协方差
|
|
out=0# 输出值
|
|
Kg=0#卡尔曼增益
|
|
Q=0.001# 过程噪声协方差
|
|
# R=.0543# 观测噪声协方差
|
|
R=.00543# 观测噪声协方差
|
|
def calc(self,value:int):
|
|
self.Now_P=self.LastP+self.Q
|
|
self.Kg=self.Now_P/(self.Now_P+self.R)
|
|
self.out=self.out+self.Kg*(value-self.out)
|
|
self.LastP=(1-self.Kg)*self.Now_P
|
|
return self.out
|
|
def my_kalman(x):
|
|
ret=[]
|
|
k=kalman()
|
|
for i in x:
|
|
ret.append(k.calc(i))
|
|
return ret
|
|
|
|
|
|
# 1阶低通滤波
|
|
def rc_low(x):
|
|
a=0.6
|
|
old=0
|
|
ret=[]
|
|
for i in x:
|
|
t=a*old+(1-a)*i
|
|
old=t
|
|
ret.append(t)
|
|
return ret
|
|
|
|
# 求导
|
|
def der(x):
|
|
old=0
|
|
ret=[]
|
|
for i in x:
|
|
t=i-old
|
|
old=i
|
|
ret.append(t)
|
|
return ret
|
|
|
|
|
|
# 滤波,过滤毛刺
|
|
def my_filter(x):
|
|
ret=[]
|
|
sub=[]
|
|
last=0
|
|
index=0
|
|
for i in x:
|
|
cat=x[index:index+10]
|
|
index+=1
|
|
lenth=len(cat)
|
|
if(lenth<10):
|
|
ret.append(0)
|
|
sub.append(0)
|
|
continue
|
|
temp=np.sort(cat)
|
|
head=np.average(cat[:lenth//2])
|
|
tail=np.average(cat[lenth//2:])
|
|
if((temp[lenth//2+1]-temp[lenth//2-1])>0.1):
|
|
if(head>tail):
|
|
last=0
|
|
else:
|
|
last=0.3
|
|
ret.append(last)
|
|
sub.append(0)
|
|
return ret,sub
|
|
|
|
|
|
# 滤波,三角变形
|
|
def my_filter2(x):
|
|
def limit(x):
|
|
x*=10
|
|
if(x>0.5):
|
|
return 0.5
|
|
elif(x<0):
|
|
return 0
|
|
else:
|
|
return x
|
|
ret=[]
|
|
sub=[]
|
|
last=0
|
|
index=0
|
|
old=0
|
|
for i in x:
|
|
index+=1
|
|
t=i-old
|
|
old=i
|
|
if(t>0.001):
|
|
last=0.3
|
|
elif(t>0.001):
|
|
last=0
|
|
ret.append(last)
|
|
sub.append(limit(t))
|
|
return ret,sub
|
|
|
|
|
|
|
|
# 只滤除毛刺
|
|
def my_burr(x):
|
|
ret=[]
|
|
last=0
|
|
index=0
|
|
for i in x:
|
|
cat=x[index:index+10]
|
|
index+=1
|
|
lenth=len(cat)
|
|
if(lenth<10):
|
|
ret.append(0)
|
|
continue
|
|
temp=np.sort(cat)
|
|
ret.append(temp[5])
|
|
return ret
|
|
|
|
|
|
|
|
|
|
|
|
# 去除基线漂移
|
|
def my_filter3(x):
|
|
x_burr=my_burr(x)
|
|
x_burr=rc_low(x_burr)
|
|
# ker=my_kalman(x_burr)
|
|
x2=der(x_burr)
|
|
x3=my_burr(x2)
|
|
ret=[]
|
|
last=0
|
|
index=0
|
|
signal=False
|
|
for i in x_burr:
|
|
if((x2[index]>=0.004)):
|
|
signal=True
|
|
elif((x2[index]<=-0.1)):
|
|
signal=False
|
|
if(signal==False):
|
|
last=i
|
|
index+=1
|
|
ret.append(i-last)
|
|
return np.multiply(x3,10),np.add(ret,0.3)
|
|
|
|
|
|
|
|
y2=add_random(y)
|
|
y3,y4=my_filter3(y2)
|
|
|
|
def show_xy(xy_list:list):
|
|
figure = plt.figure(figsize=(14,7))
|
|
ax = figure.add_axes([0.1,0.1,0.8,0.8])
|
|
index=0
|
|
for xy in xy_list:
|
|
line,=ax.plot(xy[0],xy[1],)
|
|
line.set_label("index:{d}".format(d=index))
|
|
ax.legend()
|
|
index+=1
|
|
plt.show()
|
|
# show_xy([(x,y2),(x,y3),(x,y4)])
|
|
|
|
|
|
# def calc_bytes(a,b,c,i,j,k):
|
|
# ta=((a&((1<<i)-1))<<(16-i))
|
|
# tb=((b&((1<<j)-1))<<(16-i-j))
|
|
# tc=((c&((1<<k)-1))<<(16-i-j-k))
|
|
# t=ta|tb|tc
|
|
# return bytearray([t>>8,t&0xff])
|
|
|
|
# print(calc_bytes(20,15,13,5,7,4).hex(' ')) |