飞控之卡尔曼滤波浅析

Posted 2021-02-08 traveler-wind

卡尔曼滤波

　　滤波的方法有很多种，针对不同的情况选用的最优滤波方法也是不同的。卡尔曼滤波的特点就是采用递归方法解决线性滤波问题，只需要知道当前的测量值和上一时刻的最优值，就能对此刻进行最优值计算，计算量小，不需要大量储存空间，适合性能不太强的单片机处理。二阶卡尔曼滤波更加可靠，但计算量较大，通常使用的是一阶。

现在网络上卡尔曼滤波的资料有很多，大多是一位大佬生产，说不清的码农搬砖，想要真正理解卡尔曼滤波的道理，还需静下心来从数学理论慢慢体会。分享一个正在研究的文档，比较深入的那种。

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)。