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

f4t0x

已于 2023-04-24 11:19:42 修改

阅读量536

点赞数 2

文章标签：算法

于 2023-04-24 10:53:47 首次发布

本文链接：https://blog.csdn.net/f4t0x/article/details/130337407

版权

注：代码其实没有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