一维卡尔曼滤波的python实现

1.卡尔曼滤波原理：

卡尔曼滤波是由Kalman等人于20世纪60年代提出的一种优化自回归数据处理方法，是建立在线性系统状态空间方程以及观测方程上来对系统的状态进行最优估计，主要包含状态预测和更新两个步骤。

卡尔曼滤波原理的具体讲解可以参考如下博主的讲解：
	博主讲得十分详细，十分推荐
【卡尔曼滤波器】1_递归算法_Recursive Processing】 https://www.bilibili.com/video/BV1ez4y1X7eR/?share_source=copy_web&vd_source=5e51014934b5ebce10a321714d539943

---

2.一维卡尔曼的python实现：

提示：Q是状态噪声，R是测量噪声。先模拟生成一组数，加入随机噪声后进行滤波

import numpy as np
import matplotlib.pyplot as plt
def KF(Z,over,Q,R):
    #定义超参数
    over=over
    Q=Q   #4e-4
    R=R   #0.25
    #定义尺寸函数
    cc=[over,1]
    #定义迭代的初始参数
    X_bar=np.zeros(cc)
    Xbar=np.zeros(cc)
    K=np.zeros(cc)
    P_=np.zeros(cc)
    P=np.zeros(cc)
    P[0]=1
    Xbar[0]=Z[0]
    #循环迭代
    for n in range(1,over):
        #时间更新
        X_bar[n]=Xbar[n-1]
        P_[n] = P[n-1]+Q
        #状态更新
        K[n]=P_[n]/(P_[n]+R)
        Xbar[n]=X_bar[n]+K[n]*(Z[n]-X_bar[n])
        P[n]=(1-K[n])*P_[n]
    return Xbar
    
#生成模拟测量值
over=50
Z=(24+np.sqrt(100)*np.random.normal(0,1,size=over)).reshape(over,1)
X=KF(Z,50,0.01,100)
x=np.arange(over)
fig,ax =plt.subplots()
ax.plot(x,X, label='estimate')
ax.plot(x,Z, label='measure')      
ax.legend();   

---

3.实验结果：

提示：实验中加入的噪声是服从正太分布的，所以效果不错，通过调整Q和R的大小，可以出现不同的滤波效果，若要强化滤波效果，则可以将Q值调小；若要强调跟随特性，则减少R值。

---