Python 卡尔曼滤波器实现.pdfVIP

Python卡尔曼滤波器实现

我们在设计一款新产品的时候，由于选用定制开发的一个SoC器件，导致

我们在用ADC读取经由这个SoC芯片放大后的信号时，出现了极其不稳定的情

况。正常情况下ADC读取出来的信号应当为一条平稳的直线，而现实上读取出

来的信号确上下波动极其大，远远超出了我们理论计算水平。

虽然后来通过大量的研究分析，得出时SoC极其容易受到EMI干扰，在添

加屏蔽片后成功解决了这个问题，但是在研究过程中我们发现卡尔曼滤波器在处

理这种干扰时有着媲美硬件滤波器的结果，还是感到非常惊讶。

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

现

#coding:utf-8#python3.7importnumpyimportpylab

defKalmanFilter(z,n_iter=20):

#这里是假设A=1，H=1的情况

#intialparameters

sz=(n_iter,)#sizeofarray

#Q=1e-5#processvariance

Q=1e-6#processvariance

#allocatespaceforarrays

xhat=numpy.zeros(sz)#aposteriestimateofx

P=numpy.zeros(sz)#aposterierrorestimate

xhatminus=numpy.zeros(sz)#aprioriestimateofx

Pminus=numpy.zeros(sz)#apriorierrorestimate

K=numpy.zeros(sz)#gainorblendingfactor

R=0.1**2#estimateofmeasurementvariance,changetoseeeffect

#intialguesses

xhat[0]=0.0

P[0]=1.0

A=1

H=1

forkinrange(1,n_iter):

#timeupdate

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

#measurementupdate

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

returnxhat

if__name__==__main__:

withopen((,,,encoding==)asf:

text=f.readline().split(split()

print(text)

raw_data=list()

forxintext: