卡尔曼滤波是如何工作？ 看图说话！
我务必向大家介绍卡尔曼滤波器（Kalman Filter），因为它之所能， 忒惊人！
令人惊讶的是，很少有软件工程师和科学家知道它，这实在令我伤心。因为它是如此神通而强大的工具， 用来在存在不确定性的情况下整合信息。时常，它的提取准确信息的能力几近神奇，如果你听上去认为我在故意抬高，大家可以去看我以前发布的视频（http://www.bzarg.com/p/improving-imu-attitude-estimates-with-velocity-data/），在这个视频里我演示了一个卡尔曼滤波器通过观察不确定物体的速度找出它方向。 着实精致！
卡尔曼滤波是什么？
对于一个动态系统（dynamic sytem）， 只要你对它有不确定信息（uncertain information）， 你可在任何情况下使用卡尔曼滤波器， 并且你可以训练有素的猜测（educated guess）这个系统下一步将要做什么。即使混乱的实际情况伴随并干扰了你认为的无暇的运动，卡尔曼滤波器将会常常做得很好，会弄清楚实际发生的事。并且它能利用你想不到去发掘的疯狂现象之间的相关性！
卡尔曼滤波器是连续变化（continuously changing）的系统的理想选择。它们的优点包括轻量级的存储（它们除了前一状态外不需要保存其他任何历史），并且它们非常快，使得它们非常适合于实时问题和嵌入式系统。
你从Google找到的绝大部分地方，实现卡尔曼滤波器的数学内容看上去相当可怕和模糊。这真是一个糟糕的事情，因为卡尔曼滤波器实际上超级简单和容易理解， 只要你以正确的方式来看它。因此，这就成了一个非常棒的文章话题，我会尝试用大量清晰、美观的图片和颜色来说明。要准备的先决条件其实很简单：你只需要对概率和矩阵的有基本理解。
我将先举个宽松的例子来说明卡尔曼滤波器能够解决的问题，但如果你想直接跳到诱人闪亮的图片和数学，请跳过下面一段。
用卡尔曼滤波器可以做什么呢？
举个玩具的例子：你组装了一个小机器人，可以在树林里漫步，机器人需要确切知道自己在哪里， 以便给自己导航。
我们会说机器人这时有一个状态
, 它就是一个位置和速度：
注意，状态只是一个关于你系统底层配置的一系列数字；它可以是任何东西。在我们的示例中，它指位置和速度，但它可以是关于罐中液体的量、汽车发动机的温度、用户的手指在触摸板上的位置或任何你需要追踪的事物的数据。
我们的机器人也有一个精确到约10米的GPS传感器，这很好，但它需要知道自己的位置的精度要比10米更准。在这树林里有很多沟壑和悬崖，如果机器人错误地走错几步，它就可能从悬崖上掉下来。所以仅靠GPS本身还不够好。
我们可能也知道一些关于机器人如何移动：它知道发送到轮子的电机的命令，它知道如果它朝一个方向前进并且没有干扰，在下一个时刻它可能会沿着同一个方向前进得更远。但事，当然它不会知道关于它的运动的一切：它可能被风吹打、车轮可能滑一点、或地形颠簸而翻滚；因此车轮转动的圈数可能不能准确地表示机器人实际行进的距离，并且预测不会完美。
GPS传感器告诉我们一些关于机器人状态的信息，但是只是间接地，并伴有不确定性或不准确性。我们的预测告诉我们一些关于机器人如何移动，但只是间接地，并伴有不确定性或不准确性。
但如果我们使用所有可用的信息，我们是否能得到比任何单一估计自己给出的更好的答案吗？当然，答案是肯定的，这就是卡尔曼滤波器的目标。
卡尔曼滤波器如何看你的问题的？
让我们看下下试图要理解的场景。 我们将继续使用只有位置和速度的简单状态。
我们并不知道什么实际的位置和速度;真实的位置和速度的可能组合的整个范围中，其中一些组合要比其他的更为可能：
卡尔曼滤波器假定两个变量（现在的位置和速度，在这个例子中）是随机的， 符合高斯分布。每个变量具有平均值
，这是随机分布的中心（其最有可能的状态）和方差
，这是不确定性：
在上面的图片，位置和速度是不相关（uncorrlated）的，这意味着一个变量的状态不能告诉你其他变量的可能性。
下面的例子显示了更有趣的东西：位置和速度是相关的（correlated）。观察到特定位置的可能性（likelihood）取决于你具有什么样的速度：
例如，如果我们基于旧的位置来估计新的位置，那么就可能出现上述这种情况。如果我们的速度很快，那么我们可能跑得更远，所以我们的位置就会更远。如果我们慢慢地走，我们就跑的没有那么远。
这样的相关关系是非常重要的， 要跟踪记录下来，因为它能带给了我们更多信息：一个测量值告诉我们一些其他变量的可能情况。这就是卡尔曼滤波器的目标，我们希望尽可能多地从所有不确定的测量值中挤出（squeeze）尽可能多的信息！
这种相关性可以由一种叫做协方差矩阵（covariance matrix）记录。总之，这个矩阵
的每个元素是第i个状态变量和第j个状态变量之间的相关程度。（你也许能猜测到这个协方差矩阵是对称的(symmetric)，这意味着，如果互换i和j，并没有影响）。协方差矩阵通常标记为
，因此我们将它的每个元素称为
。
用矩阵来描述问题
我们把有关状态的知识建模为一个高斯区域（Gaussian Blob），所以我们需要k时刻的两种信息：我们的最佳估计
（平均值，也称为
）及其协方差矩阵
。
（当然，在这里我们只使用了位置和速度，但是请记住， 状态可以包含任何数量变量，代表任何你想要表达的东西）。
接下来，我们需要一些手段来处理当前的状态（在k-1时刻）和在k时刻预测下一状态。记住，我们不知道哪种是“真实的”状态，但我们的预测函数不在乎。它将适用于所有的状态，并给我们一个新的分布：
我们可以用一个矩阵
表示这个预测步骤：
它将原先估计的每一个点 ，移动到一个新的预测位置，系统将执行这个移动如果原先的估计是正确的。
让我们用上这个方法。我们如何使用矩阵来预测未来下一刻的位置和速度？我们将使用一个真实的基本运动学公式：
换句说：
现在，我们有了给定下一个状态的预测矩阵，但我们仍然不知道如何更新这个协方差矩阵。
这就需要另一个公式。如果我们将分布中的每一个点乘以一个矩阵A，那么它的协方差矩阵会如何变化？
是的，很容易，我记一下来着：
所以结合公式（4）和公式(3)， 就得到如下：
外部影响
我们还没有考虑一切因素，虽然， 或许有一些变化和本身状态不相干， 但外界可能会影响到这个系统。
例如，如果这个状态是对列车的运动建模，列车操作员可能推动油门，使列车加速。类似地，在我们的机器人例子中，导航软件可能发出命令来转动车轮或停止。如果我们知道这个世界上发生了什么的额外信息，就可以把它放到一个名为
的向量中，将其添加到我们的预测中作为校正。
假设我们知道由于油门设置或控制命令的预期加速度
。从基本运动学，我们得到：
用矩阵来表示：
被称为控制矩阵和
的控制向量。（对于极为简单的系统，如果没有外部影响，你可以忽略这些）。
让我们再添加一些细节。如果我们的预测不是100％准确符合实际的模型，又该如何？
外部不确定性
一切都会很好， 如果一个状态的变化只是基于自己的特质。一切也都还好，如果状态的变化是基于外部力量，只要我们知道这些外力是怎么回事。
但是，对于我们不知道的外力呢？例如，如果我们正在跟踪一个四驱飞行器，它可能会被风吹动。如果我们跟踪一个轮式机器人，轮子可能滑动，或者在地面上的颠簸可能会让它减速。如果发生这种情况，我们并不能记录跟踪这些事情，我们的预测可能就不行了，因为我们没有考虑到这些外界的力量。
我们可以通过在每个预测步骤之后，添加一些新的不确定性，来建模与“世界”（我们难以跟踪的事物）相关的这种不确定性：
我们原来的估计每个状态可能会成为一系列状态。因为我们如此喜欢高斯区域，我们会说在
的每个点都被移动到以协方差为
的高斯区域。另一种表述为我们把难以跟踪的影响看成协方差为
的噪声。
这将产生具有不同协方差（但是具有相同的均值）的新的高斯区域：
我们通过简单地添加
得到扩展后的协方差，下面我们给出完整的预测步骤的表述：
换句话说，新的最佳估计是来自以前的最佳估计作出的预测，加上已知外部影响的校正。
并且，新的不确定性是从旧的不确定性来预测的，加上来自环境的一些额外的不确定性。
好吧，这很容易。我们有了一个对可能系统状态的估计，给定为
和
。当我们从传感器获取一些新的数据时， 又会带来什么变化？
利用测量值来改进估计值
我们可能有几个传感器能给我们关于系统状态的信息。目前，他们测量的是什么并不重要;也许一个读取位置，另一个读取速度。每个传感器告诉我们一些有关状态的间接信息， 换句话说，传感器在某种状态下会产生一组读数。
注意，读数的单位和比例可能不同于前面跟踪的状态的单位和比例。你可能想象这是个什么样子：我们把传感器建模为矩阵
。
我们可以计算出我们期望看到的传感器读数的分布：
有一件事，卡尔曼滤波非常适合处理传感器噪声。换句话说，我们的传感器都至少在某种程度上不可靠的，而且我们本来的估计每个状态，又可能会带来一系列的可能的传感器读数。
从每一个观测值，我们可能可以猜到系统所处的特定状态。但由于存在不确定性，一些状态比其他状态更有可能产生我们见到的观测值：
我们称这种不确定性的协方差（例如传感器的噪声）为
。这个分布有一个均值等于我们的观测值，我们称之为
。
所以现在我们有两个高斯区域：一个围绕着状态变化预测的平均值，一个围绕我们实际得到的传感器观测值。
我们必须设法让基于预测的状态（粉红色）与基于另外一种思路的实际看到的的传感器读数（绿色）这两者一致。
那么，我们新的最可能的状态是什么？对于任何可能的读数
，我们有两个相关的概率：
（1）传感器的观测值
是
实际/不实际的测量值的概率。
（2）
是根据我们上一个观测值预测我们应该看到的读数的概率。
如果我们有两个概率值，我们想知道， 两个都成立的概率，我们只要把他们相乘。所以，我们用了两个高斯区域，并且相乘：
我们剩下的选择在重叠区域，这个区域是两个高斯区域都是最亮最可能的区域。这比我们上面的单一估计更精确。这个交叉区域的均值会被设置成两个估计下最有可能的，因此这是囊括我们所有资料下， 对真实设置的最佳猜测。
呀 。。。这区域看起来像又一个高斯区域。
事实证明，当你将两个具有独立的均值和协方差高斯区域相乘，你会得到一个新的有自己的均值和方差矩阵的高斯区域！也许你可以看到下步如何处理：必须有一个新的公式，从旧的估计中学到新的参数！
高斯组合
让我们找下这个公式。最简单的情况是先看一维的情况。具有方差
和平均值
的1维高斯钟形曲线定义为：
我们想知道，当两个高斯曲线相乘时怎么样呢：
你可以将公式（9）代入公式（10），并做一些代数上的归一化（要注意重新归一化，使得总概率为1）
我们可以简化公式只要分解出一小块并命名它为
：
注意到事如何把你以前的估计，并添加些东西，来形成一个新的估计。看看公式是多么简单！
但是矩阵版本呢？好吧，让我们以矩阵形式重写方程（12）和（13）。如果是高斯区域的协方差矩阵，并且
各个轴合成平均值向量，则：
是一个矩阵称为卡尔曼增益（Kalman Gain），我们很快就要用它。
简单吧！我们差不多就要搞定了！
合起来看
我们有两个分布：预测的状态
和观察的测量结果
。
我们把这它们代入方程式（15），就能找到它们的重叠区域了：
我们可以在公式（16）和（17）中的每个项的前面约掉一个
(注意，
的部分前面也是隐含了它)。含有
的公式的每一项后面约掉
， 然后有如下公式。
.... 就此， 我们给出了更新步骤完整公式。
就是这样！是我们最新的最佳估计，我们可以连续循环（和
一起），轮回进入另一轮预测或者更新，重复任意多次。
包装概述一下
在上面的所有数学中，你需要实现的是公式（7）（18）和（19）。（如果你忘了它们，你可以从公式\（4）和（15）重新导出。
这将允许您更为准确为任何线性系统建模。对于非线性系统，我们使用扩展的卡尔曼滤波器（extended Kalman filter, EKF），它的工作原理是简单地线性化预测值和测量值的均值。（我或许在未来又一次写下EKF）。
如果我做得很好，希望有人在那里会意识到这是多么酷的事情，并找到一个意想不到的新地方，用上并发挥它的实力 。
