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核心代码的人了。

第二阶段、知其然

当你经历过第一阶段后，你就去了解其他方面的知识了（也有可能是，你

实在看不懂那一坨计算代码是干什么的，拼命的看了一段时间，还是没有任何

进展，这时投入产出比变得很低很低，你觉得不如先去了解下其他方面的知