Python 卡尔曼滤波器实现

去年我们在设计一款新产品的时候，由于选用定制开发的一个SoC器件，导致我们在用ADC读取经由这个SoC芯片放大后的信号时，出现了极其不稳定的情况。正常情况下ADC读取出来的信号应当为一条平稳的直线，而现实上读取出来的信号确上下波动极其大，远远超出了我们理论计算水平。

虽然后来通过大量的研究分析，得出时SoC极其容易受到EMI干扰，在添加屏蔽片后成功解决了这个问题，但是在研究过程中我们发现卡尔曼滤波器在处理这种干扰时有着媲美硬件滤波器的结果，还是感到非常惊讶。

下面介绍由Andrew D. Straw提供的基于Python语言的卡尔曼滤波滤波器实现。

#coding:utf-8  
# python3.7
import numpy  
import pylab

def KalmanFilter(z,  n_iter = 20):  
    #这里是假设A=1，H=1的情况  
      
    # intial parameters  
     
    sz = (n_iter,) # size of array   
      
    #Q = 1e-5 # process variance  
    Q = 1e-6 # process variance   
    # allocate space for arrays  
    xhat=numpy.zeros(sz)      # a posteri estimate of x  
    P=numpy.zeros(sz)         # a posteri error estimate  
    xhatminus=numpy.zeros(sz) # a priori estimate of x  
    Pminus=numpy.zeros(sz)    # a priori error estimate  
    K=numpy.zeros(sz)         # gain or blending factor  
      
    R = 0.1**2 # estimate of measurement variance, change to see effect  
      
    # intial guesses  
    xhat[0] = 0.0  
    P[0] = 1.0  
    A = 1
    H = 1

    for k in range(1,n_iter):  
        # time update  
        xhatminus[k] = A * xhat[k-1]  #X(k|k-1) = AX(k-1|k-1) + BU(k) + W(k),A=1,BU(k) = 0  
        Pminus[k] = A * P[k-1]+Q      #P(k|k-1) = AP(k-1|k-1)A' + Q(k) ,A=1  
      
        # measurement update  
        K[k] = Pminus[k]/( Pminus[k]+R ) #Kg(k)=P(k|k-1)H'/[HP(k|k-1)H' + R],H=1  
        xhat[k] = xhatminus[k]+K[k]*(z[k]-H * xhatminus[k]) #X(k|k) = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)], H=1  
        P[k] = (1-K[k] * H) * Pminus[k] #P(k|k) = (1 - Kg(k)H)P(k|k-1), H=1  
    return xhat

if __name__ == '__main__':
    with open("raw_data.txt", "r", encoding="utf-8") as f:
        text = f.readline().split(",")
    print(text)
    raw_data = list()
    for x in text:
        raw_data.append(int(x))
        print(int(x))

    xhat = KalmanFilter(raw_data, n_iter=len(raw_data))

    pylab.plot(raw_data, 'k-', label='raw measurement')  # 测量值
    pylab.plot(xhat, 'b-', label='Kalman estimate')  # 过滤后的值
    pylab.legend()
    pylab.xlabel('Iteration')
    pylab.ylabel('ADC reading')
    pylab.show()

经过卡尔曼滤波后的数据，基本上就是一条直线，与我们计算结果及我们后来添加的硬件滤波器件相一致，这也反映出卡尔曼滤波器的强大，能从众多大量噪声中找到真值，滤波器能力真不是吹的。

项目后来不用卡尔曼滤波器而大费周折采用成本更高的硬件滤波器，主要还是产品需要一个快速响应输出，而卡尔曼滤波器在这方面有点滞后了，如果对于性能要求不高的产品，这个滤波器是绝佳的利器。