ardupilot(EKF)扩展卡尔曼滤波

ardupilot(EKF)扩展卡尔曼滤波一、初识卡尔曼滤波器为了描述方便我从网上找了一张卡尔曼滤波器的5大公式的图片。

篇幅所限，下图所示的是多维卡尔曼滤波器（因为EKF2是多维扩展卡尔曼滤波器，所以我们从多维说起），为了跟好的理解卡尔曼滤波器可以百度一下，从一维开始。

这5个公式之外还有一个观测模型，根据你实际的观测量来确定，它的主要作用是根据实际情况来求观测矩阵H。

因为卡尔曼滤波器是线性滤波器，状态转移矩阵A和观测矩阵H是确定的。

在维基百科上状态转移矩阵用F表示。

在ardupilot EKF2算法中，状态转移矩阵也是用F表示的。

下面是维基百科给出的线性卡尔曼滤波器的相关公式。

上述更新（后验）估计协方差的公式对任何增益K k都有效，有时称为约瑟夫形式。

为了获得最佳卡尔曼增益，该公式进一步简化为P k|k=(I-K k H k)P k|k-1，它在哪种形式下应用最广泛。

但是，必须记住它仅对最小化残差误差的最佳增益有效。

为了使用卡尔曼滤波器来估计仅给出一系列噪声观测过程的内部状态，必须根据卡尔曼滤波器的框架对过程进行建模，这意味着指定一下矩阵：只要记住一点就行了，卡尔曼滤波器的作用就是输入一些包含噪声的数据，得到一些比较接近真是情况的数据。

比如无人机所使用的陀螺仪和加速度计的读值，他们的读值都是包含噪声的，比如明明真实的角速度是俯仰2°/s,陀螺仪的读值却是2.5°/s。

通过扩展卡尔曼之后的角速度值会变得更加接近2º/s的真实值，有可能是2.1º/s。

二、扩展卡尔曼滤波器因为卡尔曼滤波器针对的是线性系统，状态转移模型（说的白话一点就是知道上一时刻被估计量的值，通过状态转移模型的公式可以推算出当前时刻被估计量的值）和观测模型。

注：有的资料显示状态模型中有,有的没有，目前我也不清楚是为什么，有可能和被估计的对象有关。

但看多了你就会发现不管网上给的公式有怎样的不同，但总体的流程是一样的，都是这5大步骤。

我个人觉得维基百科给的公式较为标准。

因为扩展卡尔曼滤波器（EKF，Extended Kalman filter）的使用场景为非线性系统。

所以上面两公式改写为下面所示的样子，我个人的理解是，因为是非线性系统，所以没有固定的状态转移矩阵和观测矩阵。

到这儿为止卡尔曼滤波器到扩展卡尔曼滤波器的过度就完成了（多说一句，因为传感器的数据采样是有时间间隔的，算法的运行也是有间隔的，所以本文提到的KF和EKF都是离散型的）。

下面是扩展卡尔曼滤波器的相关公式。

有心的你一定发现了，扩展卡尔曼滤波器的状态转移和观测模型中，没有了状态转移矩阵和观测矩阵，但预测和更新过程中，还是要用到和。

就我个人的了解，求和的过程就是对非线性系统线性化。

和的求法如下：看到这两个求和的公式，是不是头都大了。

看了半天也不知道是什么意思，光看公式也不知道，这两个矩阵到底该怎么求。

我一开始就是这个感觉，什么雅克比矩阵，什么一阶偏导，听得头都大了（还是大学数学没学好...）。

实际上数学家最擅长的事情就是把简单的问题复杂化，把对问题的描述抽象化。

抽象带来的问题就是，公式具有高度的概括性，反映了一系列具有相同特性事物的关系，简单明了的说明了事物的本质。

但抽象化掩盖了细节，使人理解起来什么十分困难。

随后我会结合EKF2的代码作出详细的解释，参照着具体的情况和公式，反复多看几遍，你就会有一个比较清晰的了解。

想要深入了解ardupilot EKF2的原理，一定要把扩展卡尔曼滤波器的这几个公式，背的滚瓜烂熟，这样在后续读代码的时候，就能迅速联想起代码所对应的过程和公式，这一点很重要。

在了解EKF2的初期，并不需要知道扩展卡尔曼滤波器的公式为什么是这样，你只要知道EKF2的每个阶段，用的公式是什么样子的就可以了。

事实上我在为EKF2添加观测量并测试成功的两年之后，才逐渐的知道EKF2每一步所使用的公式都是什么。

以自身经验来讲，我觉得对EKF2的了解，需要经历3个阶段。

第一阶段、不知其然最开始，你想要了解ardupilot姿态解算的（ardupilot的核心代码大致分为几块，姿态解算（准确的说应该是位姿解算）、姿态控制、位置控制、任务决策、路径规划等，其中位姿解算是重中之重，其他模块都依赖于位姿解算模块所提供的准确的姿态、位置信息）相关的代码，看了它类的名字AP_NavEKF2,你在网上一搜或问了同事、同学、老师，知道了ardupilot使用的是扩展卡尔曼滤波器。

你在网上开始查资料，这时你查到的资料鱼龙混杂，有的说的是扩展卡尔曼滤波器，有的实际上说的是卡尔曼滤波器。

在查资料的过程中你渐渐的知道了四元数、欧拉角、旋转矩阵、旋转向量等名词，但此时的你还是一头雾水。

随着你看的资料越来越多，你逐渐的知道了扩展卡尔曼滤波器的作用，他的大体过程，他的输入输出是什么。

虽然这时你也许还不大清楚为啥网上查到的公式有的地方长得不大一样，但你发现他们大部分是一样的。

虽然你还不知道和是怎么求得的，对了，你有可能查到的公式里状态转移和观测矩阵是和（我当时查到的就是这样，这直接导致后面看代码更加的头雾水）。

此时，你觉得你对EKF有了一定的了解，开始看代码。

你努力的通过代码了解EKF2的程序流程，一开始是一些逻辑代码，你看得有些吃力，但还可以经过一番鏖战，你终于看到了EKF2的核心代码（在此期间，相同的代码你有可能会看好几遍，而且每一遍的理解都不一样）。

当你试着找到那5大公式时，你崩溃了。

状态转移矩阵在哪？观测矩阵在哪？是什么？又是什么？怎么还有和？公式里也没有提到呀。

你再往下看，看到了和，你的心情稍微平复了一些，终于看到两个公式里提到的变量。

但这也和理论中的公式相差十万八千里。

再往后看，你看到了函数SelectMagFusion，此时的你猜想，这个函数的作用应该是修正偏航角（Yaw）。

你觉得这个函数有可能是一个突破口，于是你在粗略的看了看这个函数之后，确定了你的想法。

之后的你了解了磁罗盘数据的格式和代表的含义，然后又开始读代码，读着读着，又看到了一大堆的计算，就在你痛苦不看之时，Kfusion出现在你的眼前。

此时的你隐约的感觉到这个函数应该是修正最终的输出量（偏航角）的地方。

在经过无数个日夜，看代码，树立猜想，再看代码，推翻猜想的循环后，你终于知道了这个函数里关键变量的作用，但此时的你还是不知道那一大坨的计算是干什么的，不过多天的思考，你知道只要找到输入，在修正量一致的情况下，计算过程应该是一致的，你沿着这个思路，修改了输入，发现输出确实发生了变化。

经过无数个日夜的煎熬，终于送了口气。

此时的你沿着这个方向，经过多次的修改、测试，终于实现了自己想要的结果。

得到预想结果的你，高兴极了。

既然修改成功了，目标达到了，EKF2里又有一大坨计算，看着都让人头疼，这个事情也可以放放了。

毕竟自己已经是修改过EKF2，ardupilot核心代码的人了。

第二阶段、知其然当你经历过第一阶段后，你就去了解其他方面的知识了（也有可能是，你实在看不懂那一坨计算代码是干什么的，拼命的看了一段时间，还是没有任何进展，这时投入产出比变得很低很低，你觉得不如先去了解下其他方面的知识）。

在学习其他知识的过程中，你还是时常想起EKF2那些你没有弄明白的问题，你时常翻看EKF2相关的代码、之前找到的EKF资料，也时不时的从网上再查看一些新的资料，还有那个matlab脚本文件（GenerateNavFilterEquations.m）。

通过注释你知道了，EKF2的代码是从这个脚本中来的，你在第一阶段中也看过这个脚本，但也没有看出来，EKF2是怎么通过这个脚本来。

也许你对matlab也不是那么熟悉（语法似乎和C++差不多，都是变量、函数，但又感觉差的很多），在看脚本的时候，看得迷迷糊糊，痛苦的很。

所有在你翻看了这些资料后，你又去了解其他的知识了。

期间你知道了ROS、了解了人工智能，知道了python的作用和应用场景，所以你花时间学习了python。

这时你还是时常想着ardupilot EKF2的实现到底是个怎样的过程。

在你不断的思考和查资料中，你似乎对EKF的了解又深了一点点。

此时你再一次的看了GenerateNavFilterEquations.m，这时你发现，你似乎可以读懂这个脚本了，这个脚本的语法和python是那么的相似，但其中有几行还是不一样，你通过百度和自己的实际操作，也逐渐的弄明白了。

在看脚本的过程中，你发现，脚本中的状态转移矩阵用的是，以某种运算通过求得了，这时你恍然大悟，EKF代码中的是和状态转移矩阵相关的。

你之前一直在找的，在代码中压根就不存在，所以你之前一直感觉EKF2和EKF理论不是那么一致。

之后，你又找到了EKF2代码中存在的、。

对应的找到了EKF理论中的哪些公式。

又花了一两周时间，弄明白了这些公式所代表的含义，将EKF2的代码和公式终于对应了起来。

但此时你又发现EKF2所用的公式和维基百科中的有一些区别，和你找的其他资料也不大一样，和秦永元老师写的《卡尔曼滤波与组合导航原理》一书中所写的公式也不一样，不是多一点就是少一点。

因为ardupilot的广泛应用，所以你的直觉告诉你，EKF2所用的公式虽然和资料里的都不一样，但事实证明它是可用的，那它这么用公式也是可以的。

为了加深你的理解，将EKF2所使用的公式，对照着代码写了一遍。

此时你已经明白了EKF2所使用的公式有哪些，状态向量、状态转移矩阵、观测矩阵和协方差矩阵都是什么。

你已经知道了在有新的传感器后，如何添加到EKF2中，使其提高系统对自身姿态、位置或速度的估计。

进行怎样的测试，来证明你的添加量是能用的。

但你此时依然不清楚EKF2所用的公式为什么是那样，换个公式行不行，EKF的哪些参数影响了它的结果，影响有多大？是如何影响的？第三阶段、知其所以然在经历了一、二阶段之后，你对EKF和其在ardupilot中的实现（EKF2）有了一定的了解，但第二阶段完成后，你还是有许多的疑问。

要解决这些疑问你必须要对EKF有更深层次的了解，知道EKF哪些公式是怎么推导来的。

这需要较深的数学基础。

第三阶段注定是个硬骨头，完成它需要很多理论知识，包括数学知识、EKF相关的知识、导航相关的知识、传感器相关的知识。

但你了解了这些知识，并用这些知识解答了EKF2为什么要选取那几个公式，那几个公式是不是最优选择的问题。

在解决了这些问题后，你可以针对多旋翼用C/C++实现一个支持IMU、GPS和磁罗盘的位姿解算算法。

如果这些事情你都做完了，恭喜你，你已经成为EKF方面的大牛了。

三、一睹EKF2芳容预测过程首先我们来确定EKF算法预测过程两个方程（预测状态估计方程和预测协方差估计方程）中的函数和变量。

也就是状态向量Xk、函数f(xk-1,uk-1,wk-1)、状态转移矩阵F、协方差矩阵P、过程噪声协方差矩阵Q和不知名的矩阵L。