卡尔曼滤波器使用一维与二维以及代码编写

注：要视频学习可以去B站搜索“DR_CAN”讲解的卡尔曼滤波器，深有体会！

链接：

1、【学习心得|基于卡尔曼滤波的MPU6050姿态解算】https://www.bilibili.com/video/BV1sL411F7fu?p=2&vd_source=3d0b47bb7325b7b3a156ba92207bbd63

2、【【卡尔曼滤波器】1_递归算法_Recursive Processing】https://www.bilibili.com/video/BV1ez4y1X7eR?vd_source=3d0b47bb7325b7b3a156ba92207bbd63

一、为啥需要卡尔曼滤波器

卡尔曼滤波器在生活中应用广泛，因为在我们生活中存在着不确定性，当我去描述一个系统，这个不确定性就包涵一下三点：

 所以要解决正样的问题，卡尔曼就诞生了。

1、下面我们进入公式推导，我们测量一个物品时，会存在测量读取误差以及测量工具误差，这时候我们正常采用多次测量取平均值来得出最终结果，那么下面我们就进行公式表达

 经过上述公式转换推导，是不是就出来了一个我们卡尔曼滤波器结果公式，其表达意思如下：

（1）既当前最优结果 = 上一次最优结果 + 卡尔曼增益*（此次测量值 - 上一次最优结果）

（2）随着测量次数的增加，当前次的测量结果就不那么重要了，反之测量次数比较少，那么当前次的结果作用就比较大。

（3）从公式也可以看出来，新的这一次X^k与上一次的估计值X^k-1有关，然后上一次的估计值X^k-1与上上次的有关，这就是一种递归思想。

（4）这也就是卡尔曼滤波器的优势，它不需要追溯很久以前的数据，只需要上一次的即可。

2、卡尔曼增益k

k在这里先做简单的解释，首先引入两个参数:

估计误差与测量误差，就是它们两个与真实值得差值

那么Kk的表达式：这里不做推导，这个公式是卡尔曼滤波器的核心公式

 

那么到这里，增益已经知道，我们把第一步的公式拿下来，合并起来

 是不是就可以转换成如下：

 3、估计误差与测量误差怎么确定参数？

估计上一次的误差同时初始化是为  kfp.Last_P = 1;切记不可为0

测量误差：这个取决于传感器的精度，如果你的传感很贵精度很厉害那这个测量误差你可以取0.1等，可以实际调试看看最终结果。

4、根据上面理解与公式，我们来做一维的滤波处理

eg:

 如果用程序表达

typedef struct

{

    float Last_P;//上次估算协方差 不可以为0 ! ! ! ! !

    float estout ;//卡尔曼滤波器输出

    float Kg;//卡尔曼增益

    float R;//(测量)观测噪声协方差

}Kalman;

Kalman kfp;

void Kalman_Init(void)

{

kfp.Last_P = 5;

kfp.estout = 40;

kfp.Kg = 0;

kfp.R = 3;//这个取决于传感器的精度

}

Void KalmanPro(float kfpMeak)

{

kfp.Kg = kfp.Last_P / (kfp.Last_P + kfp.R );

kfp.estout = kfp.estout + kfp.Kg(kfpMeak - kfp.estout);

kfp.Last_P = (1- kfp.Kg)*kfp.Last_P;

}

二、知道了一维卡尔曼处理，那么二维卡尔曼如何编写呢？

其实二维跟一维的思路与方式是一样，只不过二维是计算麻烦点变成矩阵参与运算，我相信很多同学在搜素卡尔曼滤波器陀螺仪应用时会看到大部分代码，但是不是很懂，它怎么计算这么复杂，所以在这里我们开始进行推导，一步一步，相信到最后你会发现二维跟一维其实都是一样。

在这里先上卡尔曼的五个经典公式

 按着这五个公式即可完成，具体推导代价可以看文章首页的链接在B站观看。

eg:

在这里我们以求取四轴飞行器的roll和角度为例，四轴飞行棋状元MPU6050。

（1）列状态方程

N_roll_k  =  N_roll_k-1 + Vroll * dt

N_pith_k = N_pith_k-1 + Vpith * dt

那我们将其转换成矩阵形式：

转换成矩阵形式后，我们可知 矩阵A 矩阵B，所以这就是为什么大家常说做卡尔曼要先列观测方程。

那么这一步代码编写：

N_roll_k  =  N_roll_k-1 + Vroll * dt；//求得先验值

N_pith_k = N_pith_k-1 + Vpith * dt；//求得先验值

//N_roll_k-1、 N_pith_k-1 初始值为0 

 （2）预测协方差矩阵（写方差可以看文章开头的链接B站搜索）

A的矩阵我们已经知道， 那么AT这个是A矩阵的转置，转置的意思就是将矩阵的行列位置进行对换位置，比如：

 所以AT我现在也知道了，那么还剩下Q这个矩阵，那么Q表达的意思是什么呢？

Q代表的是过程噪声协方差矩阵：

这里值0.0025是调试过程自己调试合理最佳的

左上角0.0025是指roll的方差，右下角是指pith的方差，而那两个 0 0是什么意思呢？这两个0 0是代表roll 和pith他们之间没有关系他们是相互独立的，所以就要写 0  0；

回过头假设你现在滤波的两个参数是身高跟体重，那么这个矩阵就不得写0 0，因为你想你的身高越高体重是不是也越重，所以他们两之间必会有联系。

那么 PK-1这是是指上一次的误差协方差

通常初始值我们初始化都为1，不为0，因为在后续更新迭代中P会陆续收敛，所以初始化为1即可。

现在参数都知道了，那么表达式为如下

 那么我们将矩阵看着二维数组，然后通过矩阵乘法运算可得：

 那么我们将其转换为程序：

    P[0][0] = P[0][0] +  Q[0][0];
    P[0][1] = P[0][1] +  Q[0][1];
    P[1][0] = P[1][0] +  Q[1][0];
    P[1][1] = P[1][1] +  Q[1][1];

 （3）更新卡尔曼增益

H是指单位矩阵： 1    0

                             0    1  至于为什么是这个值我也解释不来，单位矩阵就是这样

R矩阵：传感器测量噪声协方差，这个值也是自己调试的，如果你的你传感器很贵很精准这个值你就写小点，反之则写大点。这里我去0.3,

Q矩阵的 0  0跟R矩阵一样，这两个0 0是代表roll 和pith他们之间没有关系他们是相互独立的，所以就要写 0  0；

计算过程：

 

 那么矩阵的倒数怎么求呢？大家可以网上查下。

最终结果：

转换为程序： 

    K[0][0] = P[0][0] / (P[0][0] + R[0][0]);
    K[0][1] = P[0][1] / (P[0][1] + R[0][1]);  

    K[1][0] = P[1][0] / (P[1][0] + R[1][0]);
    K[1][1] = P[1][1] / (P[1][1] + R[1][1]);

（4）更新最优值

 程序编写：

roll  = N_roll_k + k[0][0]*(Z测roll  - N_roll_k )

pith = N_pith_k +k[1][1]*(Z测pith  - N_pith_k )

//因为k[0][1]  k[1][0]是等于0 的，所以上面式子就没表达了

 （5）更新协方差

    P[0][0] = (1 - k[0][0] )P[0][0] ;
    P[0][1] = 0;
    P[1][0] = 0;
    P[1][1] = (1 - k[1][1] )P[1][1] ;

  程序最终结果表达：     

  //初始化
   N_roll_k-1 = 0;
   N_pith_k-1 = 0;
   P[2][2] = {1,0,0,1};
   Q[2][2] = {0.0025,0,0,0.0025};
   R[2][2] = {0.3,0,0,0.3};
Void KalmanPro(float Z测roll,float Z测pith)
{
   N_roll_k = N_roll_k-1 + Vroll * dt；//求得先验值
   N_pith_k = N_pith_k-1 + Vpith * dt；//求得先验值
   P[0][0] = P[0][0] +  Q[0][0];
   P[0][1] = P[0][1] +  Q[0][1];//实际计算结果P[0][1] =  0;
   P[1][0] = P[1][0] +  Q[1][0];//实际计算结果P[0][1] =  0;
   P[1][1] = P[1][1] +  Q[1][1];
   K[0][0] = P[0][0] / (P[0][0] + R[0][0]);
   K[0][1] = P[0][1] / (P[0][1] + R[0][1]);//实际计算结果K[0][1] =  0; 
   K[1][0] = P[1][0] / (P[1][0] + R[1][0]);//实际计算结果K[0][1] =  0;
   K[1][1] = P[1][1] / (P[1][1] + R[1][1]);
   roll  = N_roll_k + k[0][0]*(Z测roll  - N_roll_k );
   pith = N_pith_k +k[1][1]*(Z测pith  - N_pith_k );
   N_roll_k-1 = roll;
   N_pith_k-1 = pith;
   P[0][0] = (1 - k[0][0] )P[0][0] ;
   P[0][1] = 0;
   P[1][0] = 0;
   P[1][1] = (1 - k[1][1] )P[1][1] ;
}

大家可以对比下，很直观可以发现，其实二维与一维在于两个参数有么有关系，如果没有关系相对独立，那么最终简化跟一维是一样的，所以最主要要确定好一开的观测方程。

到此卡尔曼结束。