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)])