前言    

        EKF（扩展卡尔曼滤波器）是进行IMU姿态解算的主流方法，我主要参考了论文《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU》和网路上的一些资源，对这部分内容相关公式进行推导和后面代码的走读，同时也有一些不太明白的地方，希望能和大家一起交流学习。

一些说明

按照习惯，推导过程中使用的导航坐标系为东北天坐标系（ENU），IMU的载体坐标系为载体坐标系为：右、前，上（, , ）,欧拉角的旋转顺序为：, ,  (偏航、俯仰，滚转)，另外使用的IMU型号为ADI的ADIS16470，磁力计型号为RM3100。

推导过程

0.主要框架

        主要的数据融合框架如上图所示。论文中用了所谓的两层卡尔曼滤波，首先用了陀螺仪预估出姿态角，后面的测量模型中用加速度计的测量模型和磁力的测量模型做后验估计，完成整个卡尔曼滤波的过程，其中加速度主要参与俯仰角和滚转角的后验，磁力计主要参与偏航角的后验。EKF部分的内容可以参考《概率机器人》，书中有一个比较清晰的讲解。

1.计算初始欧拉角

首先计算初始欧拉角，一般由加速度计和磁力计计算出初始欧拉角，先给出其公式：

                                                                  

 

                                                                          

 

                                         

 

                                                                          

        上面公式中, ，分别为：初始俯仰角，初始滚转角，初始偏航角。其详细的推导公式可以参考我的另外一篇博客，以下是链接：由磁场数据和加速度数据计算初始姿态角

        另外一个需要进行的步骤是把欧拉角转换为四元数，得到初始四元数，因为后续的卡尔曼滤波器中是用四元数来表示旋转，关于欧拉角到四元数的详细推导可以参考我的另外一篇博客：欧拉角转四元数公式推导，现在给出其公式：

                                           

                                           

                                           

                                           

        上式中的，，分别表示，，，即IMU的载体坐标系的，，轴的旋转角。

1.初始化噪声

        初始化过程噪声协方差矩阵和观测噪声协方差矩阵，假设各个轴之间的噪声之间是相关独立的，所以写成下面形式：

                                                                                   

                                                                                    

        上式中为陀螺仪噪声，为陀螺仪偏置噪声，为加速度计的测量噪声，为磁力计的测量噪声。

3.先验过程

        状态转移方程主要是用了四元数微分方程，其详细推导可以参考我的另外一篇博客:四元数微分方程的推导

        其公式如下：

                                                                                     

                                                                   

        以上公式是从导航坐标系到载体坐标系的四元数,四元数微分方程在连续域中的表示，因为在实际使用卡尔曼滤波器的时候，最常用的还是离散域，所以要转换到离散域，推导如下，这部分可以参考上面提到的那篇关于EKF的论文。

        状态转移方程为：

                                                                                    

        四元数微分方程可以表示为：

                                                            

 

                                                   

 

                                                                     

 

                                              

        即为状态转移矩阵的一部分，因为还要考虑到陀螺仪偏置，在我们的代码中状态变量维度为7:四元数+偏置，这部分的处理在后面的代码走读中会讲述。

        最终的状态转移矩阵为：

                                                                            

        有一点需要注意的是别忘了对状态转移方程计算出来的四元数进行归一化处理。

        设, ,则状态转移方程的雅克比矩阵求解如下：

                                                                                    

        最终的求解结果为：

                                                         

        的求解依然是借助了matlab,用matlab求偏导很方便，代码如下所示：

        先验过程的第二步为协方差计算公式，公式如下所示：