micropython版本的一阶卡尔曼滤波器

注：代码其实没有micropython的特有属性，拿到python3环境里也可以使用。

1.卡尔曼滤波器简单介绍

在做数据采集时，我们经常会需要对采集到的数据进行滤波，使数据更加平滑、贴近真实值。滤波器很多，比较经典的要属卡尔曼滤波器了。

卡尔曼滤波器的思想就是将上一刻的数据，与本次采集到的数据按照你的偏好进行加权求和，从而得出本时刻的数据，并不断迭代。最后的效果就是使得输出数据更加的平滑，接近真实值。

具体的计算方法，您可以自行百度研究，这里我只是给出我自己的一阶卡尔曼滤波器代码。

如果你不太想研究理论，又恰巧想用这个滤波器，可以直接拷贝我的代码使用。

2.一阶卡尔曼滤波算法代码及滤波效果展示

#一维卡尔曼滤波程序

class KalmanFilter:
    def __init__(self,LastP,Now_P,out,Kg,Q,R):
        #卡尔曼滤波初始化参数
        self.LastP=LastP#上次估算协方差
        self.Now_P=Now_P#当前估算协方差
        self.out=out#卡尔曼滤波器输出
        self.Kg=Kg#//卡尔曼增益
        self.Q=Q#过程噪声协方差
        self.R=R#观测噪声协方差

    
    def filt(self,mesureValue):
        #预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
        self.Now_P=self.LastP+self.Q
        #卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
        self.Kg=self.Now_P/(self.Now_P+self.R)
        #更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
        self.out=self.out+self.Kg*(mesureValue-self.out)
        #更新协方差方程: 本次的系统协方差付给 kfp->LastP 为下一次运算准备。
        self.LastP=(1-self.Kg)*self.Now_P
        return self.out        


#以下为滤波器测试代码
insKal1=KalmanFilter(LastP=16,Now_P=16,out=0,Kg=0,Q=0.1,R=1.5)

for a in [16,13,12,18,15,14,11,16,15,13,12,11,10,10,11,12,14,11,15,10]:
    kalmanTemp=insKal1.filt(a)
    print(round(kalmanTemp,2))

输出结果：

>>> %Run kalman-yiwei.py
14.64
13.83
13.17
14.61
14.72
14.54
13.69
14.23
14.41
14.08
13.61
13.01
12.33
11.8
11.62
11.7
12.23
11.95
12.64
12.04

采集数据和滤波后数据的对比图，可见滤波后数据更加平滑：

3.使用注意事项

滤波器的几个参数里面，LastP#上次估算协方差、Now_P=Now_P#当前估算协方差、out=out#卡尔曼滤波器输出、Kg=Kg#//卡尔曼增益，这四个参数在滤波过程中会不断迭代更新，所以其初始值不那么重要。

重要的是Q#过程噪声协方差、R#观测噪声协方差这两个参数，因为这两个参数直接反映了你更加偏向于上一刻的估计值还是这次的测量值。

通俗地讲，偏向于过程（也就是预测）的话，滤波结果可能在真实数据发生突变时，系统反应变迟钝。偏向于测量的话，会使输出的噪声更大，数据抖动更明显。