牢记公式，ardupilot EKF2就是纸老虎（三）！

版权声明：本文为博主原创文章，转载请附上博文链接！

三、掀开EKF2的神秘面纱

    EKF2是EKF算法在ardupilot上的代码实现。读到这里你也许已经忘了，EKF的5大公式(虽然下面是7个公式，但你将和带入公式后，就变成5个公式了)是什么了，我们先回顾下，维基百科给出的公式（公式前面的名词由谷歌翻译给出，也许有些不准确，但我目前不知道更加准确的词是什么）：

    状态转移模型和观测模型如下：

        

        

    上面这几个公式，一定要烂熟于心，下面给出EKF2使用的公式（这个几个公式是我读了GenerateNavFilterEquations.m后总结出来的）：

    预测

    预测状态估计：                                         

    预测协方差估计：                                     

    更新

    创新或计算剩余                                         

    创新（或残差）协方差                              

    接近最优的卡尔曼增益                              

    更新状态估计                                             

    更新的协方差估计                                      

    状态转移矩阵和观测矩阵被定义为以下雅克比行列式：

    

    

    矩阵和被定义为以下雅克比行列式：

    

    

    状态转移模型和观测模型如下：

    

    

    上述的13个公式，就是EKF2所使用的公式，这13个公式在维基百科中有个学名叫非加性噪声公式和方程。单看这个学名有点懵，实际他们的区别就在于状态转移模型和观测模型中噪声所在的位置。因为噪声不能直接加在和后面，只能写成和的形式，所以叫做非加性噪声公式和方程。实际上和是方程和的一种更加通用的表示方式。由于状态转移模型和观测模型的不同，EKF2锁使用的方程中，多出了矩阵和。这两个叫什么我目前也不清楚。

    在EKF2实际的应用中，过程噪声矩阵中有些元素是非加性的，有些不是。观测噪声矩阵中的元素都是加性的，所以上面的公式可以化简为如下公式：

    预测

    预测状态估计：                                         

    预测协方差估计：                                     

    更新

    创新或计算剩余                                         

    创新（或残差）协方差                              

    接近最优的卡尔曼增益                              

    更新状态估计                                             

    更新的协方差估计                                      

    状态转移矩阵和观测矩阵被定义为以下雅克比行列式：

    

    

    矩阵被定义为以下雅克比行列式：

    

    状态转移模型和观测模型如下：

    

    

    过程噪声矩阵中有些元素是非加性的，有些是加性的，所以在EKF2的实现中，预测协方差估计方程中的协方差矩阵有些元素加的是有些加的是。