卡尔曼滤波
滤波的方法有很多种，针对不同的情况选用的最优滤波方法也是不同的。卡尔曼滤波的特点就是采用递归方法解决线性滤波问题，只需要知道当前的测量值和上一时刻的最优值，就能对此刻进行最优值计算，计算量小，不需要大量储存空间，适合性能不太强的单片机处理。二阶卡尔曼滤波更加可靠，但计算量较大，通常使用的是一阶。
现在网络上卡尔曼滤波的资料有很多，大多是一位大佬生产，说不清的码农搬砖，想要真正理解卡尔曼滤波的道理，还需静下心来从数学理论慢慢体会。分享一个正在研究的文档，比较深入的那种。
https://pan.baidu.com/s/11NCpqgciVc1KIx4H66upAA
既然要真正搞懂很难，那我就反其道而行之，边用边学吧。
卡尔曼滤波由五个基本方程式组成，列出了方程式，滤波也就完成了。
深入一点的方程式从那个文章中拷进来的，看一下就好，我们分析浅显点的。
先把初始化赋值说一下，免得直接懵逼。在飞控中初始化时常用赋值如下：P(k-1)=0.02 Kg(k)=P(k)=X(k)=0 Q=0.001 R=0.5
1.预测状态方程
X(k)=A X(k-1)+B U(k) -----深
X(k)= X(k-1) ----------浅
简单的说就是把上一时刻的卡尔曼滤波的最优值 X(k-1) 乘以一个系数赋给当前预测值，这个系数常为1。U(k) 为控制增益，一般为0。
2.预测协方差方程
P(k)=A P(k-1) A’+Q -----深
P(k)= P(k-1) +Q ----------浅
P(k)是此刻系统协方差，P(k-1)是k-1时刻系统协方差，Q是系统过程噪声的协方差。在飞控中，这些值的初始化值是根据经验和计算得来的，最后统一赋一下。
3.卡尔曼增益方程
Kg(k)= P(k) H’ / (H P(k) H’ + R) -------深
Kg(k)=P(k)/（P(k)+R） ----------浅
Kg(k)叫做卡尔曼增益，P(k)是刚算出来的此刻系统协方差，R是对象测量噪声的协方差，初始化时直接给出，调试时可以根据需要调整大小，R大点时波形幅度减小，效果滞后，小点时相反，根据效果调整就好。
4.更新最优值方程
X(k)= X(k)+Kg(k) (Z(k)-H X(k)) -------深
X(k)= X(k)+Kg(k) (Z(k)- X(k)) ----------浅
X(k)就此刻的最优值，可以直接用的那种，X(k)是上一时刻的最优值，Z(k)是此刻对象的测量值，换句话说就是卡尔曼滤波函数的输入值，要是测量值都没了，还滤毛线啊，是吧。
5.更新协防差方程
P(k-1)= （I-Kg(k) H ）P(k) -------深
P(k-1)= （I-Kg(k) ）P(k) ----------浅
此刻的系统协方差一经计算变成旧的协方差，供下次使用。
这就结束了，被滤波的值是Z(k)，滤波的结果是X(k)。