卡尔曼滤波器是传感器融合工程师用于自动驾驶汽车的工具。想象一下，你有一个雷达传感器告诉你另一辆车在15米外，一个激光传感器告诉你那辆车在20米外。如何协调这些传感器测量？这就是卡尔曼滤波器的作用。卡尔曼滤波器广泛应用于自动驾驶汽车。本文介绍了卡尔曼滤波算法，希望对你有所帮助。
卡尔曼滤波算法广泛应用于控制领域。在发动机喷油控制中，扩展卡尔曼滤波理论可用于研究瞬态工况下发动机循环进气量的最优估计算法。在雷达中，人们感兴趣的是跟踪目标，但目标的测量值的位置，速度，加速度总是很吵。卡尔曼滤波器利用目标的动态信息，试图摆脱噪声的影响，得到目标位置的良好估计。
为了以后在更好的工程实践中应用卡尔曼滤波算法，今天边肖带领大家了解一下卡尔曼滤波算法的理论。
什么是卡尔曼滤波？
您可以在任何具有不确定信息的动态系统中使用卡尔曼滤波器来对系统的下一个趋势做出明智的预测。即使有各种干扰，卡尔曼滤波器也总能指出真正发生了什么。
在连续变化的系统中使用卡尔曼滤波器是理想的。它具有占用内存小(除前一状态量外无需保存其他历史数据)和速度快的优点，适用于实时问题和嵌入式系统。
在谷歌上找到的关于卡尔曼滤波实现的数学公式，大部分看起来都有点晦涩难懂，这种情况有点不好。其实如果你用正确的方式来看，卡尔曼滤波是非常简单易懂的。下面我会用漂亮的图片和颜色解释清楚。你只需要知道一些概率和矩阵的基础知识。
卡尔曼滤波能做什么？
以玩具为例：你开发了一个小机器人，可以在树林里跑来跑去。这个机器人在导航之前需要知道它的确切位置。
我们可以说机器人有一种状态，它表示位置和速度：
注意，这个状态只是一堆关于这个系统基本性质的数字。它可以是其他任何东西。在这个例子中，它是位置和速度。也可以是容器中液体的总量，汽车发动机的温度，用户的位置坐标手指在触摸面板上，或任何你需要跟踪的信号。
这个机器人有GPS，精度在10米左右，还不错。然而，它需要在10米内定位自己。森林里有许多沟壑和悬崖。如果机器人走错一步，就可能掉下悬崖，所以仅靠GPS是不够的。
也许我们知道机器人如何运动的一些信息：比如机器人知道发送给电机的指令，是否在没有任何干预的情况下向一个方向运动，在下一个状态下，机器人很可能向同一个方向运动。当然，机器人对自己的运动一无所知：可能受风的影响，车轮方向有点偏离，也可能在遇到不平的地面时翻倒。因此，车轮的转弯长度不能准确代表机器人的实际行走距离，预测也不完善。
GPS传感器告诉我们一些状态信息，我们的预测告诉我们机器人将如何移动，但它们只是间接的，并伴随着一些不确定性和不准确性。然而，如果我们使用所有可用的信息，我们能得到比任何自我评估更好的结果吗？是的，当然。那这就是卡尔曼滤波的用途。
卡尔曼滤波器怎么看你的问题？
让让我们继续一个只有两种状态的简单例子：位置和速度。
我们不我不知道实际位置和速度。它们之间有许多可能的正确组合，但有些组合比其他组合更有可能：
卡尔曼滤波器假设两个变量(在这个例子中是位置和速度)都是随机的，并且服从高斯分布。每个变量都有一个均值，代表随机分布的中心(最可能的状态)，还有一个方差，代表不确定性。
上图中，位置和速度是不相关的，也就是说不能从一个变量的状态推断出另一个变量的可能值。下面的例子更有趣：位置和速度是相关的，观察到特定位置的可能性取决于当前的速度：
可能会出现这种情况，比如我们根据旧位置估计新位置。如果速度太高，我们可能已经走远了。如果你走得慢，你就赢了。不会太远。跟踪这种关系非常重要，因为它带给我们更多的信息：其中一个测量值告诉我们其他变量的可能值，这就是卡尔曼滤波的目的，从包含不确定性的测量数据中提取尽可能多的信息！
这种相关性用协方差矩阵表示。简而言之，矩阵中的每个元素代表第I个和第j个状态变量之间的相关性。(大家可能已经猜到协方差矩阵是对称矩阵，也就是说I和J可以任意互换)。协方差矩阵通常用"其元素用"
用矩阵来描述问题。
我们基于高斯分布构建状态变量，所以在k时刻需要两条信息：最佳估计(也就是均值，在别处一般用表示)，协方差矩阵。
(1)
(当然我们这里只用位置和速度。其实这个状态可以包含多个变量，代表你想表达的任何信息)。接下来我们需要根据当前状态(时间k-1)预测下一个状态(时间k)。记住，我们不我不知道下一个状态的预测是什么true ，但我们的预测函数不我不在乎。它预测了所有的可能性，给出了新的高斯分布。
我们可以用一个矩阵来表示这个预测过程：
它将我们最初估计的每一点移动到一个新的预测位置。如果最初的估计是正确的，这个新的预测位置就是系统下一步要移动的位置。那怎么用矩阵来预测下一时刻的位置和速度呢？它由以下基本运动学公式表示：
现在，我们有了一个预测矩阵来表示下一时刻的状态，但我们仍然不我不知道如何更新协方差矩阵。此时，我们需要引入另一个公式。如果我们把分布中的每一点都乘以矩阵A，它的协方差矩阵会有怎样的变化？很简单，给出以下公式：
结合等式(4)和(3)给出：
外部控制量
我们还没有没有捕捉到所有的信息，可能有外部因素会控制系统，带来一些与系统本身状态无关的变化。
以列车的运动状态模型为例。火车司机可以操纵油门来加速火车。同样，在我们机器人的例子中，导航软件可能会给出转动或停止轮子的指令。如果我们知道这些额外的信息，我们可以用一个向量来表示它，并将其添加到我们的预测方程中进行校正。
假设由于油门或控制命令的设置，我们知道预期加速度，这可以根据基本运动学方程获得：
以矩阵的形式，它是：
称为控制矩阵和控制向量(对于没有外部控制的简单系统，这部分可以忽略)。让让我们再想一想，如果我们的预测不是100%准确，我们该怎么办？
外界干扰
如果这些状态变量根据系统而变化自身属性或者已知的外控效果，都不会有问题。
但是如果有未知的干扰呢？例如，假设我们跟踪一架四旋翼飞行器，它可能会受到风的干扰。如果我们跟踪一个轮式机器人，车轮可能会打滑，或者道路上的一个小斜坡会使它减速。这样，我们可以不要继续跟踪这些状态。如果我们不如果不考虑这些外部干扰，我们的预测就会有偏差。
在每次预测之后，我们可以添加一些新的不确定性来建立这个不确定性模型，用外面的世界(即我们没有跟踪的干扰):
原始估计中的每个状态变量更新为新状态后，仍然服从高斯分布。我们可以说每个状态变量都移动到了一个新的服从高斯分布的区域，协方差为。换句话说，我们将这些未被跟踪的扰动视为协方差为。
这产生了一个新的具有不同协方差(但相同均值)的高斯分布。
通过简单地添加扩展协方差，预测步骤的完整表达式如下所示：
从上式可以看出，新的最优估计是根据以前的最优估计进行预测的，并且加入了已知外部控制变量的修正。
新的不确定性是由之前的不确定性预测出来的，外部环境对其进行干扰。
好，我们对系统可能的趋势有一个模糊的估计，用sum表示。如果结合传感器数据会发生什么？
使用测量值来校正估计值。
我们可能有多个传感器来测量系统的当前状态。这不哪个传感器具体测量哪个状态变量并不重要。可能一个是测位置，一个是测速度。每个传感器间接地告诉我们一些状态信息。
请注意，传感器读取的数据的单位和比例可能与我们想要跟踪的状态的单位和比例不同。我们用一个矩阵来表示传感器的数据。
我们可以计算传感器读数的分布，如下面的公式和前面的表达式所示：
卡尔曼滤波的一大优点是可以处理传感器噪声。换句话说，我们的传感器或多或少是不可靠的，原始估计中的每个状态都可以对应一定范围的传感器读数。
从测得的传感器数据，我们可以大致推测出系统目前处于什么状态。但是，由于不确定性，有些状态可能比我们得到的读数更接近真实状态。
我们把这种不确定性(比如传感器噪声)表示为协方差，这种分布的均值就是我们读取的传感器数据，称为。
现在我们有两个高斯分布，一个在预测值附近，另一个在传感器读数附近。
我们必须在传感器的预测值(粉色)和测量值(绿色)之间找到最优解。
那么，我们最有可能的状态是什么？对于任何可能的读数，有两种情况：(1)传感器的测量值；(2)从前一状态获得的预测值。如果我们想知道这两种情况发生的概率，我们可以把这两个高斯分布相乘。
什么的左边是重叠部分。这个重叠部分的平均值是两个估计值的最可能值，即所有给定信息中的最佳估计值。
看啊！这个重叠区域看起来像另一个高斯分布。
可以看到，将两个均值和方差不同的高斯分布相乘，会得到一个新的均值和方差独立的高斯分布！用下面的公式解释。
融合高斯分布
第一，用一维高斯分布分析更简单。具有方差和的高斯曲线可以由以下公式表示：
两个服从高斯分布的函数相乘会得到什么？
将公式(9)代入公式(10)(注意重新归一化使得总概率为1)可以得到：
公式(11)中两个公式的相同部分由k表示：
接下来，公式(12)和(13)被进一步写成矩阵的形式。如果表示高斯分布的协方差和每个维度的均值，则：
该矩阵被称为卡尔曼增益，将在下面使用。放松点。我们你就要完成了！
合并所有公式。
我们有两个高斯分布，一个预测部分和一个测量部分，并将它们放入公式(15)中以计算它们之间的重叠部分：
从等式(14)获得的卡尔曼增益为：
公式(16)和公式(17)两边同时乘以矩阵的逆矩阵(注意是包含的)，然后公式(16)第二个方程两边同时乘以矩阵的逆矩阵，得到如下方程：
方程上给出了完整的更新步长方程。它是新的最优估计，我们可以将其和放入下一个预测和更新方程中进行连续迭代。
摘要
以上所有公式，你只需要用公式(7)、(18)、(19)。(如果忘记了，可以根据公式(4)和(15)重新推导)
我们可以使用这些公式为任何线性系统建立精确的模型。对于非线性系统，我们使用扩展卡尔曼滤波器。不同的是，EKF有一个额外的过程来线性化预测和测量部分。
标签：状态位置信息