卡尔曼滤波算法实现

    卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。
    数据滤波是去除噪声还原真实数据的一种数据处理技术,Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中,估计动态系统的状态。(百度百科)
    实际应用中,我们陀螺仪实验的时候,会发现获取的数值抖动太大,不够平滑;这样的数值在应用时会导致很多问题,因此需要通过一定的算法将数值转换的更平滑。如下采集数据:
105.87231557, 108.0076993, 85.64052734, 84.23328308, 124.71488794, 102.91086635, 96.6229862, 93.07739765, 98.52594135, 111.79610743, 94.78393208, 105.27301268, 87.23224952, 89.71036819, 117.43724425, 64.70525616, 75.48832499, 91.99249324, 105.44355624, 84.93538347
    上面的数据是获取的陀螺仪Roll方向20次样本数据,可见数值波动很大,如84都124,89到117等。下面我们将尝试卡尔曼算法,实现数值降噪滤波。
代码示例:

#!/usr/bin/env python
#coding:utf-8

'''
from JiuJiang
树莓酱的操作实例
https:://www.suhmeijiang.com
'''
import matplotlib.pyplot as plt

#滤波类
class kalman_filter:
    def __init__(self,Q,R):
        self.Q = Q
        self.R = R

        self.P_k_k1 = 1
        self.Kg = 0
        self.P_k1_k1 = 1
        self.x_k_k1 = 0
        self.ADC_OLD_Value = 0
        self.Z_k = 0
        self.kalman_adc_old=0

    def kalman(self,ADC_Value):

        self.Z_k = ADC_Value

        if (abs(self.kalman_adc_old-ADC_Value)>=60):
            self.x_k1_k1= ADC_Value*0.382 + self.kalman_adc_old*0.618
        else:
            self.x_k1_k1 = self.kalman_adc_old;

        self.x_k_k1 = self.x_k1_k1
        self.P_k_k1 = self.P_k1_k1 + self.Q
        self.Kg = self.P_k_k1/(self.P_k_k1 + self.R)

        kalman_adc = self.x_k_k1 + self.Kg * (self.Z_k - self.kalman_adc_old)
        self.P_k1_k1 = (1 - self.Kg)*self.P_k_k1
        self.P_k_k1 = self.P_k1_k1

        self.kalman_adc_old = kalman_adc

        return kalman_adc

if __name__ == '__main__':
    kalman_filter =  kalman_filter(0.001,0.1)

    #陀螺仪测试数据
    test_array = [105.87231557, 108.0076993, 85.64052734, 84.23328308, 124.71488794, 102.91086635, 96.6229862, 93.07739765, 98.52594135, 111.79610743, 94.78393208, 105.27301268, 87.23224952, 89.71036819, 117.43724425, 64.70525616, 75.48832499, 91.99249324, 105.44355624, 84.93538347, 105.87231557, 108.0076993, 85.64052734, 84.23328308, 124.71488794, 102.91086635, 96.6229862, 93.07739765, 98.52594135, 111.79610743, 94.78393208, 105.27301268, 87.23224952, 89.71036819, 117.43724425, 64.70525616, 75.48832499, 91.99249324, 105.44355624, 84.93538347]

    print(test_array)
    n = len(test_array)

    #卡尔曼过滤
    new_array=[]
    for i in range(n):
        new_array.append(int(kalman_filter.kalman(test_array[i])))

    #滤波后数据
    print(new_array)

    #图形生成展示
    plt.plot(new_array)
    plt.plot(test_array)
    plt.show()
效果如图:
执行效果
    上图可见,我们通过vnc进入可视化桌面,然后命令行执行程序文件,左侧是输出的数据;右侧是图表化滤波数据和原值数据,蓝色是滤波后数据,黄色是原值数据。可见,滤波后数据变的平滑很多。
    算法部分来源于大神文章:https://blog.csdn.net/moge19/article/details/82531119
    图形化展示部分,使用的是matplotlib,Python的2D绘图库;系统默认是没有安装的,需要自己安装,由于默认安装的matplotlib版本执行一直有问题,所以安装时指定了版本:

pip install matplotlib==2.0.2
    安装后执行如果报问题:Couldn't find foreign struct converter for 'cairo.Context',那么执行如下命令:

 sudo apt-get install python-gi-cairo
其他滤波算法,还没尝试,如果有兴趣可以试一下:十种滤波算法的Python实现。

树莓派安装VNC

VNC是一款远程桌面软件(Virtual Network Console),学名虚拟网络控制台;树莓派安装这个软件,目的是通过该软件直接桌面化访问和操作,省去了命令行操作带来的烦恼;当然也可以通过树莓派外接鼠标、键盘和显示器实现,只是成本会高一些。
登录显示效果
#下载VNC软件,这边我们通过官网下载:VNC官网,见下图根据自己的系统选择下载;
根据自己系统选择
#下载后安装,安装成功后打开软件,然后右键选择 New connection,出现新加配置页面,其中VNC server添加我们配置好的树莓派静态IP地址;Name填写我们容易区分的别名即可;然后点击OK;
双击安装后的图标,如果出现报错 “cannot currently show the desktop”,可能意味着默认分辨率需要调整,当前VNC不支持默认分辨率;可如下修改:
命令行下执行:sudo raspi-config,效果见下图,然后选择选项7 Advanced Options(高级选项)
回车确认后见下图,选择A5 Resolution (分辨率)
然后继续回车,可见下图多个分辨率列表,选择其中 1280x720 分辨率然后回车选择;
确认完之后记得最后选择 <Finish> ,完成设置。