108 lines
2.2 KiB
Python
108 lines
2.2 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.01)
|
|
y=np.sin(x)*0.5
|
|
|
|
|
|
# 添加方波
|
|
def add_line(x):
|
|
ret=[]
|
|
length=len(x)
|
|
num=length//10
|
|
index=0
|
|
for i in x:
|
|
t=(index//num)
|
|
if((t&1)!=0):
|
|
ret.append(0.1*t)
|
|
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(10):
|
|
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
|
|
|
|
# 滤波
|
|
def my_filter(x):
|
|
ret=[]
|
|
sub=[]
|
|
temp=np.sum(x[0:10])/10
|
|
t=0
|
|
t_p=0
|
|
k=0.005
|
|
signal=False
|
|
for i in x:
|
|
t_p=t
|
|
# t=t*0.9+i*0.1
|
|
t=temp
|
|
temp=(temp*9+i)/10
|
|
t_p=t-t_p
|
|
if(t_p>k):
|
|
signal=True
|
|
elif(t_p<-k):
|
|
signal=False
|
|
if(signal):
|
|
ret.append(0.3)
|
|
else:
|
|
ret.append(0)
|
|
sub.append(t_p)
|
|
return ret,sub
|
|
|
|
|
|
|
|
# 卡尔曼
|
|
class kalman:
|
|
LastP=0.02 #上次估算的协方差
|
|
Now_P=0# 当前估算的协方差
|
|
out=0# 输出值
|
|
Kg=0#卡尔曼增益
|
|
Q=0.001# 过程噪声协方差
|
|
R=.0543# 观测噪声协方差
|
|
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
|
|
|
|
y2=add_random(y)
|
|
y3,y4=my_filter(y)
|
|
|
|
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,y),(x,y3),(x,y4)])
|