在这里我就不介绍卡尔曼的数学推算了，网上的数学推导一抓一大把，如果想了解推导过程的小伙伴可以去大佬的博客。如果你是想直接简单运用卡尔曼滤波来处理mpu6050的数据，或者是处理ADC的数据，那么我希望这篇笔记可以帮助到你。
卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。
卡尔曼滤波简介：你可能经常听学长学姐提起这个算法，你是否会觉得它很普通？但实际上它是一种非常强大的算法，早在1969年，NASA（美国宇航局）就将它运用在飞船轨迹预测上了。卡尔曼滤波算法就是这样伟大而平凡，即为人类文明发展做出巨大贡献，也为普通开发者提供智囊，让我们一起感谢卡尔曼先生吧。
卡尔曼滤波的基本作用：
- 去除噪声（噪声是除有用信号以外的一切不需要的信号的总称）
- 进行最优估计
- 尽可能的还原真实值
举个栗子：比如你读陀螺仪的角度数据，我们知道陀螺仪有时会存在数据漂移的情况，它实际的角度是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
公式解释：
- 公式一：预测系统状态值
- 公式二：预测新的误差
- 公式三：计算卡尔曼增益
- 公式四：进行更新校正，得到该时刻最优值
- 公式五：更新误差协方差，为下一时刻预测误差做准备
利用公式进行滤波的步骤：
- 预测的数据值
- 测量的数据值
- 卡尔曼增益
- 最终估算的数据值
最后来看一下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.csdn.net/moge19/article/details/82531119
通俗理解卡尔曼滤波及其算法实现（实例解析）：
https://blog.csdn.net/tiandijun/article/details/72469471
卡尔曼滤波器的公式推导及示例代码编写：
https://gitchat.csdn.net/activity/5d37aeb1b5590f6f299a9cdc
