基于四元数的扩展卡尔曼(EKF)滤波器四旋翼姿态解算详解-1.KF介绍

       先说明一下，之前介绍的两种姿态解算算法http://blog.csdn.net/wkdwl/article/details/52119163    互补滤波和入梯度下降姿态解算算法，虽然在姿态解算方面效果还比较不错。但是缺点就是这两种算法并没有把传感器的噪声和系统的噪声考虑在内。因此为了进一步提高算法的精度和性能，提出了卡尔曼滤波的姿态估计算法。

       在介绍扩展卡尔曼(EKF)之前，先讲解下卡尔曼滤波(KF)。

KF滤波的模型建立以及公式推导

     KF滤波器的适用范围：线性系统。下图为KF流程图

因此可以得到KF的模型：

其中在卡尔曼滤波器中被称作预测模型，表示当前的状态受前一时刻的状态和本次的控制量的影响。A表示状态转移矩阵，是一种线性关系的矩阵。B表示系统噪声驱动阵，B形容的也是一种线性关系。和均为满足高斯分布的噪声。

在卡尔曼滤波器中被称作观测模型，表示的也是一种线性关系。该式表明在时刻，我们可以由两种方式获得当前时刻的状态估计：

1.是来源于预测模型的预测的先验状态，根据上一时刻的状态量和本次的控制量推导而来的。表示在时刻来临之前就可以获取这个状态。这个就好像我们四旋翼姿态解算中的陀螺仪微分方程，可以预测下一时刻的姿态。

2.是来源于观测模型，在时刻的测量值，来反过来更新出的当前状态。这个就像是我们从IMU传感器中读取测量值后来解算当前的姿态。

所以将两种方式得到的当前状态量融合，得到更加精确的当前状态：

其中为卡尔曼增益，为测量余差，表示实际测量值与预测测量值之间的偏移。

Kalman Filter五条黄金公式 ：

输入量（,,,）

1:

2:

3:

4:

5:

返回,

KF的推导中两个重要变量：，在t时刻的状态估计；， 在t时刻时误差协方差矩阵；

KF操作包括两个阶段：预测和校正。

预测（时间更新）：

         （预测状态）                                                                          

        （预测协方差矩阵）

校正（测量更新）：

         （计算卡尔曼增益）

        （通过测量值更新当前的估计状态）

        （更新协方差）

KF公式推导

首选输入上一时刻的已知量 ,,,,求出当前时刻的预测值和

计算卡尔曼增益$。$

$,$

,

得到：

所以得到：

这个公式说明了一个KF绝妙之处：

1.如果测量结果无误差，也既为0的时候，有,，代入预测模型得到，只剩下了测量部分。

2.如果预测值得协方差矩阵为0，既说明预测结果无误差，则只剩下预测部分。

因此，会根据测量值和预测值的信任程度来调节权重。

下面到了KF中最精华的一步了，现在我们有了对状态的预测值和协方差，同时也收集到了对状态的测量值。这时就可以通过kalman增益来计算状态估计值了。

越大表明越相信测量值，反之越相信预测值。

最后更新出当前状态的协方差矩阵

到这里相信大家基本上KF的流程差不多熟悉了，就是不断计算当前状态和协方差矩阵 来不断迭代。

下面给出框图

由此KF介绍完毕，但是在四旋翼姿态解算中，由于系统并不是线性系统。所以需要扩展卡尔曼滤波算法。在下一篇会讲到。