卡尔曼滤波学习及python仿真

在这里我就不介绍卡尔曼的数学推算了，网上的数学推导一抓一大把，如果想了解推导过程的小伙伴可以去大佬的博客。如果你是想直接简单运用卡尔曼滤波来处理mpu6050的数据，或者是处理ADC的数据，那么我希望这篇笔记可以帮助到你。

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。

卡尔曼滤波简介：你可能经常听学长学姐提起这个算法，你是否会觉得它很普通？但实际上它是一种非常强大的算法，早在1969年，NASA（美国宇航局）就将它运用在飞船轨迹预测上了。卡尔曼滤波算法就是这样伟大而平凡，即为人类文明发展做出巨大贡献，也为普通开发者提供智囊，让我们一起感谢卡尔曼先生吧。

卡尔曼滤波的基本作用：

1. 去除噪声（噪声是除有用信号以外的一切不需要的信号的总称）
2. 进行最优估计
3. 尽可能的还原真实值

举个栗子：比如你读陀螺仪的角度数据，我们知道陀螺仪有时会存在数据漂移的情况，它实际的角度是60°，但是返回的角度数据却是90°，这时用卡尔曼滤波后，可能返回的数据不是90°了而是62°或者是65°，向真实值靠近。

卡尔曼滤波最重要的是它的5大公式：

从上至下为公式1-5
含义解释：
还不会数学公式编辑，下次一定，猛哭。
x(k)^- ：k时刻系统的状态值
x(k)^ ：k时刻的最优估计值
u(k) ：外力干预
A,B : 各部分所占权重，和为1
P(k)-：k时刻与k-1时刻的状态误差
P(k)：k时刻与k-1时刻误差的协方差
Q：状态的协方差
Kk：k时刻卡尔曼增益
H：状态变换矩阵，有单位不统一时使用
R：测量时噪声的协方差
最后一个公式里面不是字母I是数字1

公式解释：

1. 公式一：预测系统状态值
2. 公式二：预测新的误差
3. 公式三：计算卡尔曼增益
4. 公式四：进行更新校正，得到该时刻最优值
5. 公式五：更新误差协方差，为下一时刻预测误差做准备

利用公式进行滤波的步骤：

1. 预测的数据值
2. 测量的数据值
3. 卡尔曼增益
4. 最终估算的数据值

最后来看一下python仿真：

import numpy as np
import matplotlib.pyplot as plt

Q = 0.00001    #噪声的协方差，也可以理解为两个时刻之间噪声的偏差
R = 0.1        #状态的协方差，可以理解为两个时刻之间状态的偏差
P_k_k1 = 1     #上一时刻状态协方差
Kg = 0         #卡尔曼增益
P_k1_k1 = 1
x_k_k1 = 0     #上一时刻状态值
ADC_OLD_Value = 0    #上一次的ADC值
kalman_adc_old = 0   #上一次的卡尔曼滤波的到的最优估计值

def kalman(ADC_Value):
    global kalman_adc_old   #定义两个全局变量，可以在程序中直接改变其中的值
    global P_k1_k1
    Z_k = ADC_Value          #测量值
    
    if (abs(kalman_adc_old-ADC_Value)>=80):      #上一状态值与此刻测量值差距过大，进行简单的一阶滤波，0618黄金比例可以随意定哦
        x_k1_k1= ADC_Value*0.382 + kalman_adc_old*0.618
    else:                 #差距不大直接使用
        x_k1_k1 = kalman_adc_old;

    x_k_k1 = x_k1_k1      #测量值
    P_k_k1 = P_k1_k1 + Q      #公式二
    Kg = P_k_k1/(P_k_k1 + R)  #公式三

    kalman_adc = x_k_k1 + Kg * (Z_k - kalman_adc_old)    #计算最优估计值
    P_k1_k1 = (1 - Kg)*P_k_k1  #公式五
    P_k_k1 = P_k1_k1     #更新状态协方差

    ADC_OLD_Value = ADC_Value   
    kalman_adc_old = kalman_adc
    return kalman_adc


plt.figure(figsize = (20,8))    #创建画布大小为（20,8）
a= [100]*200    #生成包含200个值为100的列表
array = np.array(a)

s = np.random.normal(0, 25, 200)     #生成200个符合正太分布的随机数

test_array = array + s
plt.plot(test_array,label = 'Noise')      #画出干扰噪声
adc=[]
for i in range(200):
    adc.append(kalman(test_array[i]))


plt.plot(adc,label = 'Kalman')     #卡尔曼滤波后的图像
plt.plot(array,label = 'Original')    #理想效果曲线
plt.legend()   #显示图例
plt.show()

站在巨人的肩膀上让我能快速了解卡尔曼滤波算法，感谢几位前辈给我的参考，这几篇博客都是深度好文！！！
参考博客：
利用Python实现卡尔曼滤波算法：https://blog.youkuaiyun.com/moge19/article/details/82531119
通俗理解卡尔曼滤波及其算法实现（实例解析）：
https://blog.youkuaiyun.com/tiandijun/article/details/72469471
卡尔曼滤波器的公式推导及示例代码编写：
https://gitchat.youkuaiyun.com/activity/5d37aeb1b5590f6f299a9cdc