ardupilotEKF扩展卡尔曼滤波

一、初识卡尔曼滤波器

为了描述方便我从网上找了一张卡尔曼滤波器的5大公式的图片。篇幅所限，下图所示的是多维卡尔曼滤波器(因为EKF2是多维扩展卡尔曼滤波器，所以我们从多维说起)，为了跟好的理解卡尔曼滤波器可以百度一下，从一维开始。

卡尔曼滤波器一时间更新和状态更新 ,时间更新 豆=区忆+5但L/+0 •状态更新 K*=PkH{HPkH TT斤=年+K4y一位） 这5个公式之外还有一个观测模型，根据你实际的观测量来确定，它的主要作用是根据实际情况来求观测矩阵H

因为卡尔曼滤波器是线性滤波器，状态转移矩阵A和观测矩阵H是确定的在维基百科上状态转移矩阵用F表示。在ardupilotEKF2算法中，状态转移矩阵也是用F表示的。下面是维基百科给出的线性卡尔曼滤波器的相关公式。

JH[SIR

捶打1先罚状善在计

能卜1=F正%Tt-i+Bn\"*

P*i=F*P.-IJIT【+Q

7k鹏一Hk4*i

S,-&+HtPct-jHjJ M=P.iH[Sj

心k一文中“Ki丸

P*|t=(IKtHtjPjtfc.ifl-K^)iKiRtKj步却t=zt-

7

上述更新（后验）估计协方差的公式对任何增益府都有效，有时称为约瑟 夫形式。为了获得最佳卡尔曼增益，该公式进一步简化为Pk|k=（I-KkH）Pk|k-i,

它在哪种形式下应用最广泛。但是，必须记住它仅对最小化残差误差的最佳增益有效。 为了使用卡尔曼滤波器来估计仅给出一系列噪声观测过程的内部状态，必须根据卡尔曼滤波器的框架对过程进行建模，这意味着指定一下矩阵：

・ F观察模型

・ QfrJ过程崖由酬方差： ・ R&,现察噪声的梆方差；

・ 有时日立是差制墙人模生，对于羯个时间步长心如下所述C

卡尔曼注波器槿型线定在时回*处的真实状态是根据U-1）处的壮态演变而来的

应=FAXUI+B；ut+wt 呼里

・ F『是及用于先前求念，的状态转移享空 ・ B*是控制输入虔型，应庠于控制向量u*:

・ 出火是傥设从零均值多元正态分布中福出的过程噪声.八匚有协方差，Q*：wt0M10,Q/在时间初观察：或浏

量）i§真冥状态的x6电况制成

孙=HE*+Vi 哪里

・ H七是将真实状态空间唉射到炮察空间的迎利税型

・ M0是觌测噪声，胃谈为等块值高斯白嗪同，杼方差为R*：¥上~八’（口兄）

初始发应和每个步集\"d川小…，Nv[.V*｝的曙声矢量都祓假定为用工独立

Observed Suppliedbyuser Hidden

Time=k-1

Time=k Time=k+1

卡尔聂遮波器的基册模室、正方形代表矩

阵口梯里表示多元正态分布（包痞均值和怫方差矩阵）。未计算的值是Q向量：在简单的情况下「苦料矩阵鼻时间不变，因此下后镀洲陈，但卡印晏涛波^允辞它们中的任轲一个改娈每个时间步长口

只要记住一点就行了，卡尔曼滤波器的作用就是输入一些包含噪声的数据，得到一些比较接近真是情况的数据。比如无人机所使用的陀螺仪和加速度计的读值，他们的读值都是包含噪声的，比如明明真实的角速度是俯仰2/s,陀螺

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

二、扩展卡尔曼滤波器

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

我也不清楚是为什么，有可能和被估计的对象有关。但看多了你就会发现不管网上给的公式有怎样的不同，但总体的流程是一样的，都是这5大步骤。我个 人觉得维基百科给的公式较为标准。

因为扩展卡尔曼滤波器（EKFExtendedKalmanfilter）的使用场景为非线性系统。所以上面两公式改写为下面所示的样子，我个人的理解是，因为是非线性系统，所以没有固定的状态转移矩阵和观测矩阵。到这儿为止卡尔曼滤波器到扩展卡尔曼滤波器的过度就完成了（多说一句，因为传感器的数据采样是有时间间隔的，算法的运行也是有间隔的，所以本文提到的KF和EKF都是离

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

预泅［最辑］ 预测状态估计 预测价方差估计 更新［编辑1 创新或计量剩余

离散时间预测和更新方程［靖辑］

士用A1-k-1? B|A-1=\"刑―1|山-1理+Qk 创新（或残差）协方差接贝=覆一M萩悌.J 近最优的卡尔曼携益更

新状态估计 更新的协方差估计

Sk—HkPk\\k-\\^k+Rk KA=Pfejt-i 1

工人★=比人|«\"1। 耳仲=II-K田尔2 有心的你一定发现了，扩展卡尔曼滤波器的状态转移和观测模型中，没有了状态转移矩阵F和观测矩阵H,但预测和更新过程中，还是要用到JF和曰。就我个人的了解，求F和日的过程就是对非线性系统线性化。尸和〃的求法如下：

其中状态转换和盯察达工破定义为以下雅可比行列式 理=

OX二

看到这两个求和的公式，是不是头都大了。看了半天也不知道是什么意思，光看公式也不知道，这两个矩阵到底该怎么求。我一开始就是这个感觉，什么雅克比矩阵，什么一阶偏导，听得头都大了（还是大学数学没学好...）。实际

上数学家最擅长的事情就是把简单的问题复杂化，把对问题的描述抽象化。抽象带来的问题就是，公式具有高度的概括性，反映了一系列具有相同特性事物的关系，简单明了的说明了事物的本质。但抽象化掩盖了细节，使人理解起来什么十分困难。随后我会结合EKF2的代码作出详细的解释，参照着具体的情况和公式，反复多看几遍，你就会有一个比较清晰的了解。 想要深入了解ardupilotEKF2的原理，一定要把扩展卡尔曼滤波器的这几个公式，背的滚瓜烂熟，这样在后续读代码的时候，就能迅速联想起代码所对应的过程和公式，这一点很重要。在了解EKF2的初期，并不需要知道扩展卡尔曼滤波器的公式为什么是这样，你只要知道EKF2的每个阶段，用的公式是什么样子的就可以了。事实上我在为EKF2添加观测量并测试成功的两年之后，才逐渐的知道EKF2每一步所使用的公式都是什么。

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

第一阶段、不知其然

最开始，你想要了解ardupilot姿态解算的（ardupilot的核心代码大致分为几块，姿态解算（准确的说应该是位姿解算）、姿态控制、位置控制、任务决策、路径规划等，其中位姿解算是重中之重，其他模块都依赖于位姿解算模块所提供的准确的姿态、位置信息）相关的代码，看了它类的名字AP_NavEKF2^在网上一搜或问了同事、同学、老师，知道了ardupilot使用的是犷展卡尔曼滤波器。你在网上开始查资料，这时你查到的资料鱼龙混杂，有的说的是扩展卡尔曼滤波器，有的实际上说的是卡尔曼滤波器。在查资料的过程中你渐渐的知道了四元数、欧拉角、旋转矩阵、旋转向量等名词，但此时的你还是一头雾水。随着你看的资料越来越多，你逐渐的知道了扩展卡尔曼滤波器的作用，他的大体过程，他的输入输出是什么。虽然这时你也许还不大清楚为啥网上查到的公式有的地方长得不大一样，但你发现他们大部分是一样的。虽然你还不知道和是怎么求得的，对了，你有可能查到的公式里状态转移和观测矩阵是和（我当时查到的就是这样，这直接导致后面看代码更加的头雾水）。此时，你觉得你对EKF有了一定的了解，开始看代码。你努力的通过代码了解EKF2的程序流程，一开始是一些逻辑代码，你看得有些吃力，但还可以经过一番廛战，你终于看到了EKF2的核心代码（在此期间，相同的代码你有可能会看好几遍，而且每一遍的理解都不一样）。当你试着找到那5大公式时，你崩溃

了。状态转移矩阵在哪？观测矩阵在哪？是什么？又是什么？怎么还有和？公式里也没有提到呀。你再往下看，看到了和，你的心情稍微平复了一些，终于看到两个公式里提到的变量。但这也和理论中的公式相差十万八千里。再往后看，你看到了函数SelectMagFusion,此时的你猜想，这个函数的作用应该是修正偏航角（YaW。你觉得这个函数有可能是一个突破口，于是你在粗略的看了看这个函数之后，确定了你的想法。之后的你了解了磁罗盘数据的格式和代表的含义，然后又开始读代码，读着读着，又看到了一大堆的计算，就在你痛苦不看之时，Kfusion出现在你的眼前。此时的你隐约的感觉到这个函数应该是修正最终的输出量（偏航角）的地方。在经过无数个日夜，看代码，树立猜想，再看代码，推翻猜想的循环后，你终于知道了这个函数里关键变量的作用，但此时的你还是不知道那一大坨的计算是干什么的，不过多天的思考，你知道只要找到输入，在修正量一致的情况下，计算过程应该是一致的，你沿着这个思路，修改了输入，发现输出确实发生了变化。经过无数个日夜的煎熬，终于送了口气。此时的你沿着这个方向，经过多次的修改、测试，终于实现了自己想要的结果。得到预想结果的你，高兴极了。既然修改成功了，目标达到了，EKF2里又有一大坨计算，看着都让人头疼，这个事情也可以放放了。毕竟自己已经是修改过EKF2ardupilot核心代码的人了。

第二阶段、知其然

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

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

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

么通过这个脚本来。也许你对matlab也不是那么熟悉（语法似乎和C+点不多,都是变量、函数，但又感觉差的很多），在看脚本的时候，看得迷迷糊糊，痛苦的很。所有在你翻看了这些资料后，你又去了解其他的知识了。期间你知道了ROS了解了人工智能，知道了python的作用和应

用场景，所以你花时间学习了python。这时你还是时常想着ardupilotEKF2的实现到底是个怎样的过程。在你不断的思考和查资料中，你似乎对EKF的了解又深了一点点。此时你再一次的看了GenerateNavFilterEquations.m,这时你发现，你似乎可以读懂这个脚本了，这个脚本的语法和python是那么的相似，但其中有几行还是不一样，你通过百度和自己的实际操作，也逐渐的弄明白了。在看脚本的过程中，你发现，脚本中的状态转移矩阵用的是，以某种运算通过求得了，这时你恍然大悟，EKF代码中的是和状态转移矩阵相关的。你之前一直在找的，在代码中压根就不存在，所以你之前一直感觉EKF2和EKF理论不是那么一致。之后，你又找到了EKF2代码中存在的、。对应的找到了EKF理论中的哪些公式。又花了一两周时间，弄明白了这些公式所代表的含义，将EKF2的代码和公式终于对应

了起来。但此时你又发现EKF2所用的公式和维基百科中的有一些区别，和你找的其他资料也不大一样，和秦永元老师写的《卡尔曼滤波与组合导航原理》一书中所写的公式也不一样，不是多一点就是少一点。因为ardupilot的广泛应用，所以你的直觉告诉你，EKF即用的公式虽然和资料里的都不一样，但事实证明它是可用的，那它这么用公式也是可以的。为了加深你的理解，将EKF2所

使用的公式，对照着代码写了一遍。止匕时你已经明白了EKF2所使用的公式有哪

些，状态向量、状态转移矩阵、观测矩阵和协方差矩阵都是什么。你已经知道了在有新的传感器后，如何添加到EKF2中，使其提高系统对自身姿态、位置或速度的估计。进行怎样的测试，来证明你的添加量是能用的。但你此时依然不清楚EKF2所用的公式为什么是那样，换个公式行不行，EKF的哪些参数影响了

它的结果，影响有多大？是如何影响的？

第三阶段、知其所以然

在经历了一、二阶段之后，你对EKF和其在ardupilot中的实现（EKF2有了一定的了解，但第二阶段完成后，你还是有许多的疑问。要解决这些疑问你必须要对EKF有更深层次的了解，知道EKF哪些公式是怎么推导来的。这需要较深的数学基础。第三阶段注定是个硬骨头，完成它需要很多理论知识，包括数学知识、EKF相关的知识、导航相关的知识、传感器相关的知识。但你了解了这些知识，并用这些知识解答了EKF2为什么要选取那几个公式，那几个公式是不是最优选择的问题。在解决了这些问题后，你可以针对多旋翼用C/C++

实现一个支持IMUAGPSffi磁罗盘的位姿解算算法。如果这些事情你都做完了，恭喜你，你已经成为EKF方面的大牛了。

三、一睹EKF2芳容

预测过程

首先我们来确定EKF算法预测过程两个方程（预测状态估计方程和预测协方差估计方程）中的函数和变量。也就是状态向量Xk、函数f（xk-1,uk-1,wk-

1）、状态转移矩阵F、协方差矩阵P、过程噪声协^方差矩阵Q和不知名的矩阵L其中P和Q的初值是根据相关传感器和被估计量的特性定出来的，具体为什么定的是代码中写的那些值，我目前也不清楚。

定义状态向量：

Xk总共有24个状态，分别是以旋转矢量表示的三轴角度误差、三轴速度（北东地坐标系）、三轴位置（北东地坐标系，以滤波器开始运行的点为坐标原点）、陀螺仪三轴偏差、陀螺仪三轴比例因子、加速度计Z轴偏差、三轴地

磁场（地磁场在北东地坐标系下的三轴分量）、三轴磁偏量（磁罗盘和机体坐标系没有对齐而产生的偏差，注意body_magfield表示的并不是磁罗盘测试出的三轴磁分量！！！）、两轴风速（北东地坐标系下的北和东）。四元数并不在状态向量中。

//本代码段在AP_NavEKF2_core.h中 Vector28statesArray;

structstate_elements{ Vector3fangErr;//0..2 Vector3fvelocity;//3..5 Vector3fposition;//6..8 Vector3fgyro_bias;//9..11 Vector3fgyro_scale;//12..14 floataccel_zbias;//15

Vector3fearth_magfield;//16..18 Vector3fbody_magfield;//19..21 Vector2fwind_vel;//22..23 Quaternionquat;//24..27 }&stateStruct;

定义协方差矩阵F:在voidNavEKF2_core::CovarianceInit()

定义过程噪声协方差矩阵，其中三轴角度误差、三轴速度（北东地坐标

函数中

系）、三轴位置（北东地坐标系）的噪声是非加性的，在SG中，所以24维向量processNoise的前9个元素是0。在函数NavEKF2_core::CovariancePrediction（）中

下面的一段代码出自GenerateNavFilterEquations.m,它的作用是定义了,进而推导出了状态转移矩阵、和。EKF即用到的核心公式，全部出自这个matlab脚本，各个矩阵的组成也是matlab算出来的。

首先这段脚本先定义了一大堆符号变量（通过symsdaxdaydaz'real'

这样的形式），注意这个符号变量和C/C+钠言中的变量是有区别的，符号变量就是个符号（这么说好像有点绕），不用给它赋值，这个和高中数学里面解方程一样，比如，和就是符号变量，当和等于不同值的时候得到相应的值。这些符号变量组成了EKF公式中的函数和矩阵，在EKF2中这些符号变量，变成了变量，给相应的变量赋值后，经过运算得出了卡尔曼增益和最后的状态估计向量。matlab的结果是可以包含符号变量，这个功能实在是太好用了。也许是一直使用

C++勺原因，刚看这个脚本的时候，就符号变量这一点，想了好长时间才想明白。

接下来我先说下状态向量，他的组成如下：

errRotVec=[rotErrX;rotErrY;rotErrZ];stateVector=

[errRotVec;vn;ve;vd;pn;pe;pd;dax_b;day_b;daz_b;dax_s;day_s;daz_s;dvz_b;magN;magE;magD;magX;magY;magZ;vwn;vwe]

状态向量中的每一个成员就相当于一个自变量，你可以把他们看成是。

然后是新的状态向量，其实就是由状态向量经过一些运算推导出来的。学术一点的说法就是通过一定的规则映射成。说到这里如果熟悉函数定义的你，应该已经明白了，就是。如果分开写的，（脚本定义中把角度误差、速度、位置分别写成了3维向量的格式）中的成员就是。