扩展卡尔曼滤波器(EKF)：一个面向初学者的交互式教程翻译

1、扩展卡尔曼滤波器教程在使用OpenPilot和Pixhawk飞控时，经常遇到扩展卡尔曼滤波（EKF）。从不同的网页和参考论文中搜索这个词，其中大部分都太深奥了。所以我决定创建自己学习教程。本教程从一些简单的例子和标准（线性）卡尔曼滤波器，通过对实际例子来理解卡尔曼滤波器。Part1:一个简单的例子想象一个飞机准备降落时，尽管我们可能会担心许多事情，像空速、燃料、等等，当然最明显是关注飞机的高度（海拔高度）。通过简单的近似，我们可以认为当前高度是之前的高度失去了一小部分。例如，当每次我们观察飞行高度时，认为飞机失去了2%的高度，那么它的当前高度是上一时刻高度的98%:altitude=0.98*

2、altitudecurrent_timeprevious_time工程上对上面的公式，使用“递归”这个术语进行描述。通过递归前一时刻的值，不断计算当前值。最终我们递归到初始的“基本情况”，比如一个已知的高度。试着移动上面的滑块，看看飞机针对不同百分比的高度变化。QP-U一一一VTimePart2:处理噪声当然，实际从传感器比如GPS或气压计获得测量高度时，传感器的数据或多或少有所偏差。如果传感器的偏移量为常数，我们可以简单地添加或减去这偏移量来确定我们的高度。不过通常情况下，传感器的偏移量是一个时变量，使得我们所观测到的传感器数据相当于实际高度加上噪声:observed_altitude=al

3、titude+noisecurrent_timecurrent_timecurrent_time试着移动上面的滑块看到噪声对观察到的高度的影响。噪音被表示为可观测的海拔范围的百分比。0%noiseTime10%noiseQprrlll-或PWAJotKqoTimeTimePart3:全部考虑所以现在我们有两个方程描述我们的飞机的状态:altitude=0.98*altitudecurrent_timeprevious_timeobserved_altitude=altitude+noisecurrent_timecurrent_timecurrent_time这些方程是很容易理解，但他们不够通

4、用处理一般系统，除了我们上面所举的例子。更一般的方程，工程上采用熟悉的像X、y和z为变量，a和b为常量，下标k代表时间。所以我们的方程变成:-Jk=HJk+*X是我们系统的当前状态，xk-1是其先前的状态，a是一常数（在我们的示例中是0.98），zk是我们当前的观测变量，vk是当前的测量噪声。卡尔曼滤波如此受欢迎的原因之一是，通过给定的观测量zk,常量a以及整体的测量噪声vk,它可以让我们得到一个对当前状态的准确的估计值xk。我们还应该考虑到，飞机实际的高度变化可能没有描述的那么光滑。飞机通常在下降过程中是会经历一定的动荡的。这种动荡是通过定义的噪声来描述，可视为另一个噪声信号:altitud

5、e=0.98*altitude+turbulencecurrent_timeprevious_timecurrent_time一般表述为Wk表示过程噪声，比如飞机下降过程的动荡，这是一个固有模型的一部分，而不是观测量或测量值。为了方便其他问题的讨论，我们将忽略过程噪声一段时间，但我们将在传感器融合一节继续讨论它。Part4:状态估计这里再次（忽略过程噪声）列出我们的两个方程，来描述我们正在观察的系统状态:IJt=代H*_l3=Tfc亠Vk因为我们的目标是从观测量z获得状态量x,我们可以重写第二个方程为:当然，问题是我们不知道当前的噪声Vk,理论上它是不可预测的。幸运的是，卡尔曼滤波器能够使我们

6、通过考虑当前的观测值和先前的估计值来估计当前的状态值。工程上在变量上面使用“”帽子符号表示估计值。所以小表示当前状态的估计值。然后我们可以用先前的估计值和当前观测值的权重来表示当前的估计值。ijt=ijt-i+一ijt-i）这里的g表示权重值的增益。这个方程用红色突出显示，是因为这就是我们直接使用的卡尔曼滤波器。现在，这一切看起来相当复杂，想象一下如果gk的取极值会如何？当gk=0时，我们得到：ifc=ifc-i+0（魁ifc-1）=ifc-i也就是说增益为0时，观测值对状态估计没有影响，当前状态级先前的状态估计值。当gk=1时，我们得到：ifc=ifc-1+1（抵ifc-1）=ifc-1+抵

7、一ifc-1=抵也就是说，当增益为1时，前面的状态估计值并不重要，我们所获取的当前状态估计完全跟当前的观测值有关。当然，实际的增益值可能介于这两种极端情况之间。试着移动下面的滑块，看到增益对获得当前状态估计的影响:Zk=105gjt=0-5ijt=107.50Tfc_i=110g=105gjt=0.85Tjt=105.750Part5:计算增益所以现在我们有一个公式，可以根据先前的估计值心i,当前的观测量，当前的增益处来计算当前状态估计心。ijfc=ifc-i+佻（魁-ijk-i）那么，我们如何计算增益呢？答案是：间接地从噪声获得。回想下，每一个观测值都与一个特定的噪声值有关：-Jt=+班我们

8、不知道每一个观测量的单独的噪声值，但我们通常能知道噪声的平均值。比如：一个传感器所发布的精度能告诉我们大概的输出的噪声值。称这个变量为r，r没有下标值，因为r不随时间而变化，与传感器的性能有关。我们可以根据r计算当前增益gk：9k=Pk-l/（pk-l+）这里的pk是一个用于递归计算的预测误差：Pfc=（19k）pk-l让我们思考之前这两个公式是什么意思。假设我们之前预测的误差讥|是零。然后我们获得当前的增益为m,这样下一个状态估计与当前的状态估计没有不同。这是有道理的，因为当预测值准确的时候，我们不应该调整状态估计。在另一个极端，如果预测误差是1。然后将获得1卫+厂）。如果r是零，或者传感器

9、的噪声很小，那我们的增益为1。新的状态估计心与观测值眛有很大关系。但随着r增大，增益越来越小，换句话说，当传感器的噪声太大，则忽略一个糟糕的预测值。第三个公式，递归地从它的前一个值pk-1和当前增益gk计算预测误差pk?当川-，我们得到“n。所以，对于状态估计，0增益表示没有更新的预测误差。当以=1,我们得到川。因此，增益的最大值也就是没有预测误差，当前的观测值直接用于更新当前状态。Part6:预测与更新我们现在基本可以运行卡尔曼滤波器，并看到一些结果。首先，你可能会思考常量a在我们的原始方程中有什么作用：似乎在我们的状态估计方程中消失了：it=ifc-i+-ijt-i)答案是，我们需要这两个

10、方程一起来估计状态。事实上，这两个方程基于不同类型的信息，表示状态的估计。我们的原始方程表示预测状态应该是什么，第二个方程表示基于观测值的更新预测。所以我们重写原始方程：it=慈djk-i最后，我们使用常数a表示预测误差：Pk=紀PJt-l住这两个红色公式表示我们的卡尔曼滤波器的预测阶段。周期预测/更新的思路是，预测/更新，重复等尽可能多的时间步长。Part7:运行滤波器Predict.ijt=rtijt-iPh=叩Jt_i佗=+）ik=ik+佻（8一ijt）PJt=（1gk）pk为了滤波器的运行，我们需要：一个观测值序列状态估计的初始值（基本情况）心。这可以是我们的第一个观测值。预测误差的初

11、始值皿。它不能是0否则pk将永远与0相乘。所以我们随意将其设置为1。对于我们的观测量，我们将伪造观测量，而不是试图测量一个实际系统（如飞机准备降落时）在观测量基础上添加随机噪声vk的范围在（-200+-200），理想值1，初始一旦你准备好运行滤波器，点击运行按钮，看看卡尔曼滤波器产生光滑的曲线（绿色），带噪声信号（红色），以及非常接近于原始干净的信号（蓝色）。也可以尝试不同的值。k0123456789100075056342231623717813310075*9376904723703044311012607611210.5610.3150.1770.10.0560.0310.0180.01

12、0.00693770352739529622216712594700.0030.0020.0010000001000r=200a=0.75InitializeRunPart8:一个更真实的模型回忆描述系统的这两个方程:Ifc=代HJt-l-Jt=Ifc+雄xk是我们系统的当前状态，xk-1是它之前的状态，a是一个常数，zk是我们当前的观测值，vk是当前的观测噪声（不准确）。虽然这两个方程很好地应用于各种各样的系统，但有时不能完全的表达系统特征。首先，我们没有实时的控制飞行器，例如飞行员控制飞机那样，控制飞行器前后飞行。为了控制该系统，我们引入一个变量uk,代表了当前的控制信号，跟飞行员给飞机发

13、的指令一样。正如前面的状态xk-1扩展了一个比例系数a,这种控制信号也可以由一个常量表示，称之为b。所以我们完整的状态方程为：Tfc=rtTfc+加吒一般来说，任何信号以外的噪声可以用常系数来表示，所以我们观察zk的方程可以改写:Part9:修改估计值这里我们获得更真实的/更一般的方程的状态和观测变量系统：Ifc=aifc-1+buk抵=czjfc+w正如我们所料，引入这些新组件到我们的模型，需要相应修改预测和更新方程:Predict.iodate.ifc=aifc-1+buPk=apfc-ifffc=PkC/(cPkc+r)ifc=ifc+g貞抵-cifc)Pfc=(1-9kC)Pk这里有一

14、个扩展我们的飞机演示，添加控制信号表示飞行员稳步提高控制序列提高飞机的高度。试着移动滑块来调整常量的值不同。在前面的演示，原始信号显示蓝色，观测信号用红色，绿色Kalman-filtered信号。a=0.95c=1b=0Resetr=100Part10:添加速度量到系统回忆我们的飞机的原始高度方程:alt2tudecurrent=0.98+aliuleprevious一般形式表达成：HJk=怨瑰一1想想以前学的数学和物理，这种公式似乎有点奇怪。高度，毕竟是一种距离（海拔或高出地面），我们学会了公式distance-velocity*fjjm?我们可以协调这两种不同的方式思考距离？答案是肯定的，

15、但它需要我们采取两个步骤。首先，我们需要将当前时间和以前时间的概念引入到我们上面的公式，并思考在离散时间的步骤，而不是总体距离:distancecurrent-distanceprevious+velociprevious*currenttilBGpreviou换句话说，我们现在是之前的公式加上离散时刻的距离。如果我们执行此计算，以固定的采样周期（一秒,100纳秒,六个月,等等），然后我们可以简化成:distancecurrent-distancepreirous+veDcitypreir:ious+tjjnestep这个方程我们接近我们的一般形式IJt=佗HJt-l但是我们仍然似乎有两种截然

16、不同的表达方式:一个是简单的形式，另一个包括简单的和形式。第二步，采样线性代数提出一个一般方程。Part11:线性代数所以我们有一个方程表示距离的速度和时间:distanceeurrent-distanceprevious+velocityprevious+tiestep我们试着用更一般的形式来表达。幸运的是，数学家们很久以前就发明了“一个怪异的技巧”，以同样的方式表示这两种方程。诀窍就是把系统的状态不是一个数字表达，而是一个数字列表称为向量，就像在Excel电子表格的一列。向量的大小（元素）的数量对应于我们想设置的状态量的数量。在我们这个例子中，用两个状态量，距离和速度：di.stancek

17、veloci.tyk在这里当前状态被定义为一个向量包含当前距离和流速。所以这是如何帮助我们？好吧，我们从线性代数学到的另一件事是矩阵。如果一个向量就像一列的值在一个电子表格，那一个矩阵就像整个表格。当我们用一个矩阵乘以一个向量得到相同的另一个向量大小:XfiH4-.y._ct+dy_abcd例如：3143向量和矩阵可以是任何大小，只要他们匹配:abcar駅+軌r+czdefV!=di+ey+fz.5hi_+hy-iz_我们也可以将两个矩阵相乘得到另一个矩阵:ftbcvstnr+iru+ctas+bv+cyfit-bw-ezdef-u2w+eu+fxds+ev+fydt+ew-1-fz.9Hi-

18、一工y2_gr+Jm+ixgs+hv-iygt-hut-iz_两个矩阵相加简单，只是每一对元素相加:_abcTst4-rb4-sc-1def+UVw=d-1-ue4-v/+U1.9hi-_XyjIh-yi-z_回到手头的任务中，我们定义了一个矩阵卫1wlestep-o1.使用一个大写字母来表示一个矩阵。然后我们的一般方程几乎是相同的:HJt=工作原理：di-stancek1tiniestepdi.stfmcek_i_veloci.tyk_.01._velocityk-i_1+di.stancck-1+timestep+veloci.tyk-i0*disttmcek-i+1*velocity-1

19、d?.stance-l+timestep*veloci.tyk-i_velod.tyk-1_换句话说，当前距离是前一刻的距离加上前一刻的速度乘以采样周期，当前速度与前一刻的速度相同。如果我们想要模型表达系统随着时间的推移的速度变化，我们可以很容易地修改向量和矩阵包括加速度:di.stfmcek1tirnestep0_di-stfmcek-1di.stfmcek-lveloci.tyk01timestepveloci.tyk-1velod.tyk-1_acceleration_001_acceleratiorik-i_accelerationk；-i_Part12:重新审视预测和更新这里是我们修

20、改系统状态的公式:x是一个向量，A是一个矩阵。您可能还记得，这个方程的原始形式Uk是一个控制信号，b是系数。观测方程:3=CTfc4-VkZk是测量（观察、传感器）信号，和vk是一些噪音，造成传感器信号的不准确。那么，我们如何修改这些原始形式与我们的新向量矩阵的方法？您可能会怀疑，线性代数这很简单:我们写大写字母系数b和c,使它们矩阵而不是标量值：IJt=卫亠Bl蚯3=Cijt4-那么所有的变量（状态、观察、噪声、控制）被认为是向量，我们有一组矩阵向量乘法。那么我们的预测和更新方程呢？回想一下，它们是：Predict.ifc=aifc-l+臥Pk=fipk-1fiUpdate9h=Pfcc/（

21、qjfcc+r）ijt=it+佻（魁一tit）pjt=（1gkc）pk我们想利用所有的常量a，b，c和r，使他们成为矩阵A、B、C和R就万事大吉了。但事情有点棘手。你可能还记得答应解释为什么我没有将简化成川I。答案是，线性代数乘法不像普通简单的乘法。我们经常必须按照一定的顺序执行乘法。例如，A*B不一定等于B*A。我们可能还需要转置矩阵，表示成把矩阵旁边的小上标T。更换一个矩阵是通过把每一行成一列，每列成一行。下面是一些例子：记住这些知识，我们可以重写预测方程如下：ifc=卫蘇_1+BukPk=Apk-lA1注意，我们已经重写了大写字母A和Pk,表明他们是矩阵。我们已经知道为什么A是一个矩阵，

22、我们将推迟一会儿理解为什么Pk也是一个矩阵。我们的更新方程呢？第二个更新方程，更新状态估计x-k,很简单：ijt=ijt+一Udjt)但是第一个方程涉及：9k=PkC/(cpjtc+r)除法改成乘法表示：9k=Pkc(cpkc+r)_1现在我们把c，r，g和常数1变成成矩阵：Gk=PkCT(CPkCTPk=(I-GQR那么，我们如何计算EQ，是矩阵心+冈的逆？就像普通的反演1=1-，我们需要矩阵求逆，乘以一个矩阵的逆产生单位矩阵：这里，因为是3*3矩阵，所以：Part13:传感器融合所以现在我们有一个完整我们的卡尔曼滤波方程的线性代数(向量,矩阵)形式：lodelH*=TiTfc-l+Bl蚯-

23、Jt=+VkPredict.ik=卫B盅Pk=APiA1iodate.Gk=PkCT(CPkCTR)-1ijt=ijt+Gk(h-Cik)瑰=(IGkC)Pk看起来需要添加很多外部传感器来满足我们所设置的状态量，实际上，使用卡尔曼滤波器即可，称为传感器融合。回到我们的飞机的例子中，我们注意到飞行员会获得更多的信息（观测），而不仅仅是高度；他们也有仪表来显示飞机的空速，地速，航向，经纬度，室外温度等。想象一个飞机只有三个传感器，每一种都对应于一个给定的状态量:高度的气压计，罗盘航向，空速的空速管。暂时假设这些传感器是完全准确的（没有噪声）。我们的观测方程：altitudek-1Jieadingk

24、-1airspeed-1变成：barometerkcompasspitotk现在假设我们有另一个传感器的高度信息，比如GPS。空速管和GPS将受到高度的影响，所以我们的方程就变成:注：我们的C矩阵是有点不切实际，值可能不是I。问题的关键是，一个矩阵中的非零值对应于一个传感器和一个状态之间的关系。这里我们有我们的第一个例子，系统不是一个简单的传感器和状态值之间一一对应。任何此类系统提供我们传感器融合的机会；也就是说，可以组合来自多个传感器的读数（气压计、GPS）来推断一些关于一个状态（高度）的信息。我们的直觉告诉我们，最好有一个以上的一些重要的信息来源。在下一节中，我们将看到与单独使用一个传感器

25、相比，卡尔曼滤波器使用传感器融合如何给我们一个更好的状态估计。Part14:传感器融合例子为了解传感器融合是如何工作的，让我们再次限制自己的系统只有一个状态。进一步简化，我们将假设我们的状态转换关系模型（矩阵）没有信息，所以必须只依赖于传感器的值。也许跟我们使用两个不同的温度计测量室外温度一样。所以我们就将我们的状态转换矩阵设为1缺乏一个状态转换关系的模型为我们的温度计，我们只是假设当前状态与前面相同的状态。因为要传感器融合，所以在我们的观测向量zk中当然会需要超过一个传感器值，在这个例子中，我们可以将其视为当前我们两个温度计的读数。我们假设这两个温度传感器同样有助于我们评估，所以我们的C矩阵只是一对1：抵=Cxk+%=1ifc+vjt预测和更新方程需要的三个矩阵（A、C、R）我们已经有两个矩阵（A、C）了。那么，我们如何获得R？回想一下，我们的单传感器的示例中，我们定义r作为观测噪声信号的方差vk。系统拥有超过两个传感器，R是一个包含每一对传感器之间的协方差矩阵。这个矩阵的对角线上的元素将每个传感器的r值，即传感器与本身的方差。对角线的元素代表一个传