卡尔曼滤波

关于卡尔曼滤波器有很多不同的文章，但是很难找到包含所有滤波公式的解释的文章。 我认为，如果不了解，这门科学将变得完全不可理解。 在本文中，我将尝试以一种简单的方式来解释一切。

卡尔曼滤波器是用于过滤各种数据的非常强大的工具。 其背后的主要思想是应该使用有关物理过程的信息。 例如，如果要从汽车的速度计中过滤数据，则其惯性使您有权将较大的速度偏差视为测量误差。 卡尔曼滤波器也很有趣，因为它在某种程度上是最好的滤波器。 我们将精确地讨论它的含义。 在文章的最后，我将展示如何简化公式。

初赛

首先，让我们记住概率论中的一些定义和事实。

随机变量

当有人说给它一个随机变量时 $$$\ xi$ ，则表示它可能采用随机值。 不同的值具有不同的概率。 例如，如果有人掷出骰子，则该组值是离散的 $$$\ {1,2,3,4,5,6 \}$ 。 当您处理移动粒子的速度时，显然您应该使用一组连续的值。 每次实验（测量）后得出的值，我们用 $$$x_1，x_2，...$ ，但有时我们会使用与随机变量相同的字母 $$$\ xi$ 。 在连续的一组值的情况下，随机变量的特征在于其概率密度函数 $$$\ rho（x）$ 。 此函数显示随机变量落入较小邻域的可能性 $$$dx$ 点的 $$$x$ 。 正如我们在图片上看到的，此概率等于图表下方阴影线矩形的面积 $$$\ rho（x）dx$ 。

在我们的生活中，当概率密度为时，随机变量通常具有高斯分布。 $$$\ rho（x）\ sim e ^ {-\ frac {（x- \ mu）^ 2} {2 \ sigma ^ 2}}$ 。

我们可以看到钟形功能 $$$\ rho（x）$ 以该点为中心 $$$\亩$ 其特征宽度约为 $$$\ sigma$ 。
因为我们在谈论高斯分布，所以不提它来自何处将是一个罪过。 以及数量 $$$e$ 和 $$$\ pi$ 扎根于数学并可以在最意想不到的地方找到，因此高斯分布在概率论中具有深厚的渊源。 以下引人注目的陈述部分解释了许多过程中高斯分布的存在：
让一个随机变量 $$$\ xi$ 具有任意分布（实际上对任意性有一些限制，但根本没有限制）。 表演吧 $$$n$ 实验并计算总和 $$$\ xi_1 + ... + \ xi_n$ 的价值下降。 让我们做很多实验。 显然，每次我们将获得不同的总和值。 换句话说，这个和是一个具有自己的分布规律的随机变量。 事实证明，对于足够大的 $$$n$ ，该和的分布定律趋于高斯分布（顺便说一下，钟形特征宽度 $$$\ sqrt n$ 。 在Wikipedia： 中央极限定理中阅读更多内容。 在现实生活中，有许多值是大量独立且分布均匀的随机变量的总和。 因此，此值具有高斯分布。

含义值

根据定义，随机变量的平均值是在我们进行越来越多的实验并计算下降值的平均值时达到极限的值。 平均值用不同的方式表示：数学家用 $$$E \ xi$ （期望），物理学家用 $$$\上线{\ xi}$ 或 $$$<\ xi>$ 。 我们将把它表示为数学家。

例如，高斯分布的平均值 $$$\ rho（x）\ sim e ^ {-\ frac {（x- \ mu）^ 2} {2 \ sigma ^ 2}}$ 等于 $$$\亩$ 。

方差

对于高斯分布，我们清楚地看到随机变量趋于落入其平均值的某个区域内 $$$\亩$ 。 让我们再次享受高斯分布：

在图片上，您可能会看到值大多下降的区域的特征宽度为 $$$\ sigma$ 。 我们如何估计任意随机变量的宽度？ 我们可以绘制其概率密度函数的图形，而只是在视觉上评估特征范围。 但是，最好选择一种精确的代数方式进行此评估。 我们可能会发现偏离平均值的平均长度： $$$E | \ xi-E \ xi |$ 。 该值是对特性偏差的良好估计 $$$\ xi$ 。 但是，我们非常了解，在公式中使用绝对值是有问题的，因此在实践中很少使用此公式。

一种更简单的方法（从计算的角度来看很简单）是计算 $$$E（\ xi-E \ xi）^ 2$ 。

此值称为方差，用 $$$\ sigma_ \ xi ^ 2$ 。 方差的平方根是对随机变量特征偏差的良好估计。 称为标准偏差。

例如，可以计算出高斯分布 $$$\ rho（x）\ sim e ^ {-\ frac {（x- \ mu）^ 2} {2 \ sigma ^ 2}}$ 方差等于 $$$\ sigma ^ 2$ 因此标准偏差为 $$$\ sigma$ 。 这个结果确实符合我们的几何直觉。 实际上，这里隐藏了一些小作弊。 实际上，在高斯分布的定义中，您可以看到数字 $$$2$ 在表达的分母中 $$$-\ frac {（x- \ mu）^ 2} {2 \ sigma ^ 2}$ 。 这个 $$$2$ 出于标准偏差而故意站在那里 $$$\ sigma_ \ xi$ 完全等于 $$$\ sigma$ 。 因此，高斯分布的公式是以某种方式编写的，请记住要计算其标准偏差。

独立随机变量

随机变量可能相互依赖。 想象一下，您将一根针扔在地板上并测量其两端的坐标。 这两个坐标是随机变量，但它们相互依赖，因为它们之间的距离应始终等于针的长度。 如果第一个的下降结果不依赖于第二个的结果，则随机变量彼此独立。 对于两个自变量 $$$\ xi_1$ 和 $$$\ xi_2$ 其乘积的平均值等于其乘积的平均值： $$$E（\ xi_1 \ cdot \ xi_2）= E \ xi_1 \ cdot \ xi_2$

卡尔曼滤波器

问题陈述

让用 $$$x_k$ 我们打算测量然后过滤的值。 它可以是坐标，速度，加速度，湿度，温度，压力等

让我们从一个简单的例子开始，这将引导我们提出一般性问题。 想象一下，您有一台只能向前和向后行驶的无线电遥控玩具车。 了解了系统的质量，形状和其他参数后，我们已经计算出控制操纵杆如何作用于汽车的速度 $$$v_k$ 。


汽车的坐标将由以下公式表示

$$

$$x_ {k + 1} = x_k + v_kdt$$

在现实生活中，我们无法获得精确的坐标公式，因为一些小小的干扰会影响汽车，例如风，颠簸，道路上的石头，因此汽车的真实速度将与计算得出的速度不同。 所以我们添加一个随机变量 $$$\ xi_k$ 在最后一个方程的右侧：

$$

$$x_ {k + 1} = x_k + v_kdt + \ xi_k$$

我们在汽车上还有一个GPS传感器，该传感器试图测量汽车的坐标 $$$x_k$ 。 当然，这种测量存在误差，这是一个随机变量 $$$\ eta_k$ 。 因此，从传感器中我们将得到错误的数据：

$$

$$z_k = x_k + \ eta_k$$

我们的目的是为真实坐标找到一个好的估计 $$$x_k$ ，知道错误的传感器数据 $$$z_k$ 。 这个好的估计我们将用 $$$x ^ {opt}$ 。
总的来说坐标 $$$x_k$ 可以代表任何值（温度，湿度，...），我们将用以下方式表示的控制成员 $$$u_k$ （以汽车为例 $$$u_k = v_k \ cdot dt$ ） 坐标和传感器测量的等式如下：

（1）

让我们讨论一下，我们在这些方程式中知道什么。

• $$$u_k$ 是控制系统演进的已知值。 我们确实从系统模型中知道这一点。
• 随机变量 $$$\ xi$ 代表系统模型中的误差和随机变量 $$$\ eta$ 是传感器的错误。 它们的分布规律不依赖于时间（取决于迭代索引） $$$k$ ）
• 误差均值为零： $$$E \ eta_k = E \ xi_k = 0$ 。
• 我们可能不知道随机变量的分布规律，但是我们知道它们的方差 $$$\ sigma_ \ xi$ 和 $$$\ sigma_ \ eta$ 。 请注意，方差不取决于时间（在 $$$k$ ），因为相应的分配法则都没有。
• 我们假设所有随机错误彼此独立：当时的错误 $$$k$ 不依赖于当时的错误 $$$k’$ 。

请注意，过滤问题不是平滑问题。 我们的目的不是要平滑传感器的数据，我们只想获得尽可能接近真实坐标的值 $$$x_k$ 。

卡尔曼算法

我们将使用归纳法。 想象一下，在步骤 $$$k$ 我们已经找到过滤后的传感器的值 $$$x ^ {opt}$ ，这是对真实坐标的良好估计 $$$x_k$ 。 回想一下，我们知道控制实坐标的方程式：

$$

$$x_ {k + 1} = x_k + u_k + \ xi_k$$

因此，在获取传感器的值之前，我们可能会声明它会显示接近 $$$x ^ {opt} + u_k$ 。 不幸的是，到目前为止，我们还不能说出更精确的内容。 但是在这一步 $$$k + 1$ 我们会从传感器获得不精确的读数 $$$z_ {k + 1}$ 。
卡尔曼的思想如下。 为了获得最佳的真实坐标估计 $$$x_ {k + 1}$ 我们应该在非精确传感器的读数之间获得一个黄金中间 $$$z_ {k + 1}$ 和 $$$x ^ {opt} + u_k$ -我们的预测，就是我们期望在传感器上看到的。 我们会给一个重量 $$$K$ 到传感器的值 $$$（1-K）$ 到预测值：

$$

$$x ^ {opt} _ {k + 1} = K \ cdot z_ {k + 1} +（1-K）\ cdot（x_k ^ {opt} + u_k）$$

系数 $$$K$ 被称为卡尔曼系数。 这取决于迭代索引，严格来说，我们应该写 $$$K_ {k + 1}$ 。 但是为了使公式保持良好的形状，我们将省略索引 $$$K$ 。
我们应该以估计坐标的方式选择卡尔曼系数 $$$x ^ {opt} _ {k + 1}$ 尽可能接近真实坐标 $$$x_ {k + 1}$ 。
例如，如果我们确实知道我们的传感器非常精确，那么我们会相信它的读数并给他很大的重量（ $$$K$ 接近一个）。 相反，如果传感器根本不精确，那么我们将依靠我们的理论预测值 $$$x ^ {opt} _k + u_k$ 。
在一般情况下，我们应将估算误差最小化：

$$

$$e_ {k + 1} = x_ {k + 1} -x ^ {opt} _ {k + 1}$$

我们使用方程式（1）（位于蓝框上）来重写错误的方程式：

$$

$$e_ {k + 1} =（1-K）（e_k + \ xi_k）-K \ eta_ {k + 1}$$

现在该进行讨论了，“最小化错误”这个表达是什么意思？ 我们知道该错误是一个随机变量，因此每次使用不同的值。 实际上，这个问题没有唯一的答案。 同样，在随机变量的方差情况下，当我们尝试估计其概率密度函数的特征宽度时。 因此，我们将选择一个简单的标准。 我们将最小化平方的均值：

$$

$$E（e ^ 2_ {k + 1}）\ rightarrow min$$

让我们重写最后一个表达式：

$$

$$E（e ^ 2_ {k + 1}）=（1-K）^ 2（E_k ^ 2 + \ sigma ^ 2_ \ xi）+ K ^ 2 \ sigma ^ 2_ \ eta$$

当其导数为零时，最后一个表达式取最小值。 因此，当：

$$

$$\ displaystyle K_ {k + 1} = \ frac {Ee ^ 2_k + \ sigma ^ 2_ \ xi} {Ee ^ 2_k + \ sigma ^ 2_ \ xi + \ sigma ^ 2_ \ eta}$$

在这里，我们用下标来写卡尔曼系数，因此我们强调它的确取决于迭代步骤。 我们用方程式代替均方误差 $$$E（e ^ 2_ {k + 1}）$ 卡尔曼系数 $$$K_ {k + 1}$ 使其价值最小化：

$$

$$\ displaystyle E（e ^ 2_ {k + 1}）= \ frac {\ sigma ^ 2_ \ eta（Ee ^ 2_k + \ sigma ^ 2_ \ xi）} {Ee ^ 2_k + \ sigma ^ 2_ \ xi + \ sigma ^ 2_ \ eta}$$

因此，我们已经解决了我们的问题。 我们得到了计算卡尔曼系数的迭代公式。

例子

在本文开头的情节中，从虚构的GPS传感器中过滤出了基准数据，该数据固定在虚构的汽车上，并以恒定的加速度运动 $$$一$ 。

$$

$$x_ {t + 1} = x_t + at \ cdot dt + \ xi_t$$

分析方法

如果看一下卡尔曼系数 $$$K_k$ 迭代中的变化 $$$k$ ，有可能看到它稳定在一定值 $$$K_ {刺}$ 。 例如，如果传感器和模型的均方误差相对于彼此为十比一，则来自迭代步骤的卡尔曼系数依赖性图将如下所示：


在下一个示例中，我们将讨论如何简化生活。

第二个例子

在实践中，碰巧我们几乎不了解物理模型中所过滤的内容。 想象一下，您决定从自己喜欢的加速度计中过滤掉这些测量值。 实际上，您根本不知道加速度计将如何移动。 您可能唯一知道的是传感器误差的方差 $$$\ sigma ^ 2_ \ eta$ 。 在这个难题中，我们可能会将物理模型中的所有未知信息放入随机变量中 $$$\ xi_k$ ：

$$

$$x_ {k + 1} = x_k + \ xi_k$$

严格来说，这种系统不满足我们对随机变量施加的条件 $$$\ xi_k$ 。 因为它保存着运动物理对我们来说未知的信息。 我们不能说误差在不同的时刻彼此独立，并且均值等于零。 换句话说，这意味着在这种情况下不应用卡尔曼理论。 但是无论如何，我们可以尝试通过选择一些合理的值来使用卡尔曼理论的机制 $$$\ sigma_ \ xi ^ 2$ 和 $$$\ sigma_ \ eta ^ 2$ 得到一个很好的过滤数据图。
但是有一种更简单的方法。 我们看到随着步骤的增加 $$$k$ 卡尔曼系数始终稳定在一定值 $$$K_ {刺}$ 。 因此，不用猜测系数的值 $$$\ sigma ^ 2_ \ xi$ 和 $$$\ sigma ^ 2_ \ eta$ 并计算卡尔曼系数 $$$K_k$ 通过困难的公式，我们可以假定该系数为常数，并仅选择该常数。 该假设不会对过滤结果产生太大影响。 首先，无论如何，卡尔曼机制并不完全适用于我们的问题，其次，卡尔曼系数迅速稳定为常数。 最后，一切变得非常简单。 我们几乎不需要卡尔曼理论中的任何公式，只需要选择一个合理的值 $$$K_ {刺}$ 并将其插入迭代公式中

$$

$$x ^ {opt} _ {k + 1} = K_ {stab} \ cdot z_ {k + 1} +（1-K_ {stab}）\ cdot x_k ^ {opt}$$

在下一张图表上，您可以看到来自假想传感器的两种不同测量方法的滤波结果。 第一种方法是诚实的方法，其所有公式均来自卡尔曼理论。 第二种方法是简化方法。


我们看到，这两种方法之间没有太大差异。 当卡尔曼系数仍然不稳定时，开始时会有很小的变化。

讨论区

我们已经看到，卡尔曼滤波器的主要思想是选择系数 $$$K$ 过滤后的值

$$

$$x ^ {opt} _ {k + 1} = Kz_ {k + 1} +（1-K）（x ^ {opt} _k + u_k）$$

平均而言，将尽可能接近真实坐标 $$$x_ {k + 1}$ 。 我们看到过滤后的值 $$$x ^ {opt} _ {k + 1}$ 是传感器测量的线性函数 $$$z_ {k + 1}$ 和之前的过滤值 $$$x ^ {opt} _k$ 。 但是以前的过滤值 $$$x ^ {opt} _k$ 本身是传感器测量的线性函数 $$$z_k$ 和之前的过滤值 $$$x ^ {opt} _ {k-1}$ 。 依此类推，直到链结束。 因此，滤波后的值线性地取决于所有先前传感器的读数：

$$

$$x ^ {opt} _ {k + 1} = \ lambda + \ lambda_0z_0 + \ ldots + \ lambda_ {k + 1} z_ {k + 1}$$

这就是将卡尔曼滤波器称为线性滤波器的原因。 有可能证明卡尔曼滤波器是所有线性滤波器中最好的。 从某种意义上讲最好的是，它最小化了误差的均方根。

多维案例

可以将所有卡尔曼理论推广到多维情况。 公式看起来有些复杂，但推导公式的想法仍然与一维相同。 例如，在这个漂亮的视频中，您可以看到示例。

文学作品

卡尔曼（Kalman）撰写的原始文章可以在这里下载：
http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf