滤波器之RLS滤波器(python实现)

关于原理的讲解这篇文章说的比较清楚
https://blog.csdn.net/HJ199404182515/article/details/52504150

#xk:信号输入 dk:期望信号 lambd：遗忘因子 M:滤波器阶数 c:正数 保证矩阵P非奇异
#err:保存误差 wn:保存权值 yn:滤波输出 kn:中间变量
"""
def rls(xk,dk,lambd,M,c):
    #Init
    sig_len = len(xk) - M + 1
    I = np.eye(M)                                      
    yn = np.zeros(sig_len)
    errn = np.zeros(len(xk))
    wn = np.zeros((sig_len,M))
    #Loop
    for i in range(0,sig_len):
        if i == 0:
            p_old = I/c
            w_old = np.zeros(M)
        d = dk[i]
        x = np.flipud(xk[i:i+M])
        xt = np.reshape(x,(-1,1)).T            #x的转置
        k = np.dot(p_old,x)/(lambd +np.dot(np.dot(xt,p_old), x))
        y = np.dot(xt,w_old)                   #FIR滤波器输出
        err = d - y                            #估计误差     
        ket = k * err
        w = w_old + ket                        #计算滤波器系数矢量  
        p = np.dot((I - np.dot(np.reshape(k,(-1,1)),xt)),p_old)/lambd
        #更新
        p_old = p
        w_old = w
        #滤波结果存储
        yn[i] = y
        errn[i] = err
        wn[i] = np.reshape(w,(-1,1)).T
   return yn,errn,wn