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

《扩展卡尔曼滤波器(EKF)：一个面向初学者的交互式教程-翻译.pdf》由会员分享，可在线阅读，更多相关《扩展卡尔曼滤波器(EKF)：一个面向初学者的交互式教程-翻译.pdf（19页珍藏版）》请在金锄头文库上搜索。

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

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

3、传感器数据相当于实际高 度加上噪声: observed_altitudecurrent_time = altitudecurrent_time + noisecurrent_time 试着移动上面的滑块看到噪声对观察到的高度的影响。 噪音被表示为可观测的海拔范围的百 分比。 Part 3:全部考虑全部考虑 所以现在我们有两个方程描述我们的飞机的状态: altitudecurrent_time = 0.98 * altitudeprevious_time observed_altitudecurrent_time = altitudecurrent_time + noisecurrent_tim

4、e 这些方程是很容易理解，但他们不够通用处理一般系统，除了我们上面所举的例子。更一般 的方程，工程上采用熟悉的像 x、y 和 z 为变量，a 和 b 为常量，下标 k 代表时间。所以我 们的方程变成: x 是我们系统的当前状态，xk1 是其先前的状态，a 是一常数(在我们的示例中是 0.98)，zk 是我们当前的观测变量，vk 是当前的测量噪声。卡尔曼滤波如此受欢迎的原因之一是，通 过给定的观测量 zk，常量 a 以及整体的测量噪声 vk，它可以让我们得到一个对当前状态的 准确的估计值 xk。 我们还应该考虑到， 飞机实际的高度变化可能没有描述的那么光滑。 飞机通常在下降过程中 是会经历一定的

5、动荡的。这种动荡是通过定义的噪声来描述，可视为另一个噪声信号: altitudecurrent_time = 0.98 * altitudeprevious_time +turbulencecurrent_time 一般表述为 Wk 表示过程噪声，比如飞机下降过程的动荡，这是一个固有模型的一部分，而不是观测量 或测量值。为了方便其他问题的讨论，我们将忽略过程噪声一段时间，但我们将在传感器融 合一节继续讨论它。 Part 4:状态估计状态估计 这里再次(忽略过程噪声)列出我们的两个方程，来描述我们正在观察的系统状态: 因为我们的目标是从观测量 z 获得状态量 x，我们可以重写第二个方程为: 当然

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

7、得到： 也就是说，当增益为 1 时，前面的状态估计值并不重要，我们所获取的当前状态估计完全跟 当前的观测值有关。当然，实际的增益值可能介于这两种极端情况之间。试着移动下面的滑 块，看到增益对获得当前状态估计的影响: Part 5:计算增益计算增益 所以现在我们有一个公式， 可以根据先前的估计值， 当前的观测量， 当前的增益 来计算当前状态估计。 那么，我们如何计算增益呢？答案是：间接地从噪声获得。回想下，每一个观测值都与一个 特定的噪声值有关： 我们不知道每一个观测量的单独的噪声值，但我们通常能知道噪声的平均值。比如：一个传 感器所发布的精度能告诉我们大概的输出的噪声值。称这个变量为 r，r

8、没有下标值，因为 r 不随时间而变化，与传感器的性能有关。我们可以根据 r 计算当前增益 gk： 这里的 pk 是一个用于递归计算的预测误差: 让我们思考之前这两个公式是什么意思。 假设我们之前预测的误差是零。然后我们获得当前的增益为，这 样下一个状态估计与当前的状态估计没有不同。这是有道理的，因为当预测值准确的时候， 我们不应该调整状态估计。在另一个极端，如果预测误差是 1。然后将获得。如 果 r 是零，或者传感器的噪声很小，那我们的增益为 1。新的状态估计与观测值有 很大关系。但随着 r 增大，增益越来越小，换句话说，当传感器的噪声太大，则忽略一个糟 糕的预测值。 第三个公式，递归地从它的

9、前一个值 pk1 和当前增益 gk 计算预测误差 pk？当，我 们得到。所以，对于状态估计，0 增益表示没有更新的预测误差。当， 我们得到。因此，增益的最大值也就是没有预测误差，当前的观测值直接用于更新 当前状态。 Part 6:预测与更新预测与更新 我们现在基本可以运行卡尔曼滤波器，并看到一些结果。首先，你可能会思考常量 a 在我们 的原始方程中有什么作用： 似乎在我们的状态估计方程中消失了： 答案是，我们需要这两个方程一起来估计状态。事实上，这两个方程基于不同类型的信息， 表示状态的估计。 我们的原始方程表示预测状态应该是什么， 第二个方程表示基于观测值的 更新预测。所以我们重写原始方程：

10、 最后，我们使用常数 a 表示预测误差： 这两个红色公式表示我们的卡尔曼滤波器的预测阶段。周期预测/更新的思路是，预测/更 新，重复等尽可能多的时间步长。 Part 7:运行滤波器运行滤波器 为了滤波器的运行，我们需要： 一个观测值序列 状态估计的初始值(基本情况) 。这可以是我们的第一个观测值。 预测误差的初始值。它不能是 0，否则 pk 将永远与 0 相乘。所以我们随意将其设 置为 1。 对于我们的观测量， 我们将伪造观测量， 而不是试图测量一个实际系统(如飞机准备降落时)， 在观测量基础上添加随机噪声 vk 的范围在(-200 + -200)，理想值，初始 状态： 一旦你准备好运行滤波器

11、，点击运行按钮，看看卡尔曼滤波器产生光滑的曲线(绿色)，带噪 声信号(红色)，以及非常接近于原始干净的信号(蓝色)。也可以尝试不同的值。 Part 8:一个更真实的模型一个更真实的模型 回忆描述系统的这两个方程: xk 是我们系统的当前状态， xk1 是它之前的状态， a 是一个常数， zk 是我们当前的观测值， vk 是当前的观测噪声(不准确)。 虽然这两个方程很好地应用于各种各样的系统，但有时不能完全的表达系统特征。首先，我 们没有实时的控制飞行器，例如飞行员控制飞机那样，控制飞行器前后飞行。为了控制该系 统，我们引入一个变量 uk，代表了当前的控制信号，跟飞行员给飞机发的指令一样。正如

12、前面的状态 xk1 扩展了一个比例系数 a，这种控制信号也可以由一个常量表示，称之为 b。 所以我们完整的状态方程为： 一般来说，任何信号以外的噪声可以用常系数来表示，所以我们观察 zk 的方程可以改写： Part 9:修改估计值修改估计值 这里我们获得更真实的/更一般的方程的状态和观测变量系统： 正如我们所料，引入这些新组件到我们的模型，需要相应修改预测和更新方程: 这里有一个扩展我们的飞机演示， 添加控制信号表示飞行员稳步提高控制序列提高飞机的高 度。试着移动滑块来调整常量的值不同。在前面的演示，原始信号显示蓝色，观测信号用红 色，绿色 Kalman-filtered 信号。 Part 1

13、0:添加速度量到系统添加速度量到系统 回忆我们的飞机的原始高度方程: 一般形式表达成： 想想以前学的数学和物理，这种公式似乎有点奇怪。高度，毕竟是一种距离(海拔或高出地 面)，我们学会了公式 我们可以协调这两种不同的方式思考距离？答案是肯定的， 但它需要我们采取两个步骤。 首 先， 我们需要将当前时间和以前时间的概念引入到我们上面的公式， 并思考在离散时间的步 骤，而不是总体距离: 换句话说，我们现在是之前的公式加上离散时刻的距离。如果我们执行此计算，以固定的采 样周期(一秒,100 纳秒,六个月,等等)，然后我们可以简化成: 这个方程我们接近我们的一般形式 但是我们仍然似乎有两种截然不同的表

14、达方式:一个是简单的形式，另一个包括简单的和形 式。第二步，采样线性代数提出一个一般方程。 Part 11:线性代数线性代数 所以我们有一个方程表示距离的速度和时间: 我们试着用更一般的形式来表达。 幸运的是， 数学家们很久以前就发明了 “一个怪异的技巧” ， 以同样的方式表示这两种方程。 诀窍就是把系统的状态不是一个数字表达， 而是一个数字列表称为向量， 就像在 Excel 电子 表格的一列。 向量的大小(元素)的数量对应于我们想设置的状态量的数量。 在我们这个例子 中，用两个状态量，距离和速度： 在这里当前状态被定义为一个向量包含当前距离和流速。 所以这是如何帮助我们？好吧， 我们从线性代

15、数学到的另一件事是矩阵。 如果一个向量就像 一列的值在一个电子表格， 那一个矩阵就像整个表格。 当我们用一个矩阵乘以一个向量得到 相同的另一个向量大小: 例如： 向量和矩阵可以是任何大小，只要他们匹配: 我们也可以将两个矩阵相乘得到另一个矩阵: 两个矩阵相加简单，只是每一对元素相加: 回到手头的任务中，我们定义了一个矩阵 使用一个大写字母来表示一个矩阵。然后我们的一般方程几乎是相同的: 工作原理： 换句话说， 当前距离是前一刻的距离加上前一刻的速度乘以采样周期， 当前速度与前一刻的 速度相同。 如果我们想要模型表达系统随着时间的推移的速度变化， 我们可以很容易地修改 向量和矩阵包括加速度: P

16、art 12: 重新审视预测和更新重新审视预测和更新 这里是我们修改系统状态的公式: x 是一个向量，A 是一个矩阵。您可能还记得，这个方程的原始形式 uk 是一个控制信号，b 是系数。观测方程： Zk 是测量(观察、传感器)信号，和 vk 是一些噪音，造成传感器信号的不准确。那么，我们 如何修改这些原始形式与我们的新向量矩阵的方法？您可能会怀疑，线性代数这很简单:我 们写大写字母系数 b 和 c，使它们矩阵而不是标量值: 那么所有的变量(状态、观察、噪声、控制)被认为是向量，我们有一组矩阵向量乘法。那么 我们的预测和更新方程呢？回想一下，它们是: 我们想利用所有的常量 a，b，c 和 r，使他们成为矩阵 A、B、C 和 R 就万事大