在网上看了不少与卡尔曼滤波相关的博客、论文,要么是只谈理论、缺乏感性,或者有感性认识,缺乏理论推导。能兼顾二者的少之又少,直到我看到了国外的一篇博文,真的惊艳到我了,不得不佩服作者这种细致入微的精神,翻译过来跟大家分享一下,原文链接:http://www./p/how-a-kalman-filter-works-in-pictures/ 什么是卡尔曼滤波? 你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波,对系统下一步的走向做出有根据的预测,即使伴随着各种干扰,卡尔曼滤波总是能指出真实发生的情况。 我们能用卡尔曼滤波做什么? 用玩具举例:你开发了一个可以在树林里到处跑的小机器人,这个机器人需要知道它所在的确切位置才能导航。 我们可以说机器人有一个状态 ,表示位置和速度: 注意这个状态只是关于这个系统基本属性的一堆数字,它可以是任何其它的东西。在这个例子中是位置和速度,它也可以是一个容器中液体的总量,汽车发动机的温度,用户手指在触摸板上的位置坐标,或者任何你需要跟踪的信号。 或许我们知道一些机器人如何运动的信息:例如,机器人知道发送给电机的指令,知道自己是否在朝一个方向移动并且没有人干预,在下一个状态,机器人很可能朝着相同的方向移动。当然,机器人对自己的运动是一无所知的:它可能受到风吹的影响,轮子方向偏了一点,或者遇到不平的地面而翻倒。所以,轮子转过的长度并不能精确表示机器人实际行走的距离,预测也不是很完美。 卡尔曼滤波是如何看到你的问题的 下面我们继续以只有位置和速度这两个状态的简单例子做解释。 我们并不知道实际的位置和速度,它们之间有很多种可能正确的组合,但其中一些的可能性要大于其它部分: 卡尔曼滤波假设两个变量(位置和速度,在这个例子中)都是随机的,并且服从高斯分布。每个变量都有一个均值 μ,表示随机分布的中心(最可能的状态),以及方差 ,表示不确定性。 在上图中,位置和速度是不相关的,这意味着由其中一个变量的状态无法推测出另一个变量可能的值。下面的例子更有趣:位置和速度是相关的,观测特定位置的可能性取决于当前的速度: 这种情况是有可能发生的,例如,我们基于旧的位置来估计新位置。如果速度过高,我们可能已经移动很远了。如果缓慢移动,则距离不会很远。跟踪这种关系是非常重要的,因为它带给我们更多的信息:其中一个测量值告诉了我们其它变量可能的值,这就是卡尔曼滤波的目的,尽可能地在包含不确定性的测量数据中提取更多信息! 使用矩阵来描述问题 我们基于高斯分布来建立状态变量,所以在时刻 k 需要两个信息:最佳估计 (即均值,其它地方常用 μ 表示),以及协方差矩阵 。 (当然,在这里我们只用到了位置和速度,实际上这个状态可以包含多个变量,代表任何你想表示的信息)。接下来,我们需要根据当前状态(k-1 时刻)来预测下一状态(k 时刻)。记住,我们并不知道对下一状态的所有预测中哪个是“真实”的,但我们的预测函数并不在乎。它对所有的可能性进行预测,并给出新的高斯分布。 我们可以用矩阵 来表示这个预测过程: 它将我们原始估计中的每个点都移动到了一个新的预测位置,如果原始估计是正确的话,这个新的预测位置就是系统下一步会移动到的位置。那我们又如何用矩阵来预测下一个时刻的位置和速度呢?下面用一个基本的运动学公式来表示: 现在,我们有了一个预测矩阵来表示下一时刻的状态,但是,我们仍然不知道怎么更新协方差矩阵。此时,我们需要引入另一个公式,如果我们将分布中的每个点都乘以矩阵 A,那么它的协方差矩阵 会怎样变化呢?很简单,下面给出公式: 结合方程(4)和(3)得到: 外部控制量 我们并没有捕捉到一切信息,可能存在外部因素会对系统进行控制,带来一些与系统自身状态没有相关性的改变。 以矩阵的形式表示就是: 称为控制矩阵,称为控制向量(对于没有外部控制的简单系统来说,这部分可以忽略)。让我们再思考一下,如果我们的预测并不是100%准确的,该怎么办呢? 外部干扰 如果这些状态量是基于系统自身的属性或者已知的外部控制作用来变化的,则不会出现什么问题。 原始估计中的每个状态变量更新到新的状态后,仍然服从高斯分布。我们可以说的每个状态变量移动到了一个新的服从高斯分布的区域,协方差为。换句话说就是,我们将这些没有被跟踪的干扰当作协方差为的噪声来处理。 这产生了具有不同协方差(但是具有相同的均值)的新的高斯分布。 我们通过简单地添加得到扩展的协方差,下面给出预测步骤的完整表达式: 由上式可知,新的最优估计是根据上一最优估计预测得到的,并加上已知外部控制量的修正。 用测量值来修正估计值 我们可能会有多个传感器来测量系统当前的状态,哪个传感器具体测量的是哪个状态变量并不重要,也许一个是测量位置,一个是测量速度,每个传感器间接地告诉了我们一些状态信息。 注意,传感器读取的数据的单位和尺度有可能与我们要跟踪的状态的单位和尺度不一样,我们用矩阵 来表示传感器的数据。 我们可以计算出传感器读数的分布,用之前的表示方法如下式所示: 卡尔曼滤波的一大优点就是能处理传感器噪声,换句话说,我们的传感器或多或少都有点不可靠,并且原始估计中的每个状态可以和一定范围内的传感器读数对应起来。 从测量到的传感器数据中,我们大致能猜到系统当前处于什么状态。但是由于存在不确定性,某些状态可能比我们得到的读数更接近真实状态。 我们将这种不确定性(例如:传感器噪声)用协方差表示,该分布的均值就是我们读取到的传感器数据,称之为。 我们必须在预测值(粉红色)和传感器测量值(绿色)之间找到最优解。 剩下的就是重叠部分了,这个重叠部分的均值就是两个估计最可能的值,也就是给定的所有信息中的最优估计。 如你所见,把两个具有不同均值和方差的高斯分布相乘,你会得到一个新的具有独立均值和方差的高斯分布!下面用公式讲解。 融合高斯分布 先以一维高斯分布来分析比较简单点,具有方差 和 μ 的高斯曲线可以用下式表示: 如果把两个服从高斯分布的函数相乘会得到什么呢? 将式(9)代入到式(10)中(注意重新归一化,使总概率为1)可以得到: 将式(11)中的两个式子相同的部分用 k 表示: 下面进一步将式(12)和(13)写成矩阵的形式,如果 Σ 表示高斯分布的协方差, 表示每个维度的均值,则: 矩阵称为卡尔曼增益,下面将会用到。放松!我们快要完成了! 将所有公式整合起来 我们有两个高斯分布,预测部分,和测量部分,将它们放到式(15)中算出它们之间的重叠部分: 由式(14)可得卡尔曼增益为: 将式(16)和式(17)的两边同时左乘矩阵的逆(注意里面包含了 )将其约掉,再将式(16)的第二个等式两边同时右乘矩阵 的逆得到以下等式: 上式给出了完整的更新步骤方程。就是新的最优估计,我们可以将它和放到下一个预测和更新方程中不断迭代。 总结 以上所有公式中,你只需要用到式(7)、(18)、(19)。(如果忘了的话,你可以根据式(4)和(15)重新推导一下) (ps: 第一次用Markdown,添加图片和公式心累啊,什么时候能直接拖拽就好了~~) 附Markdown使用技巧: |
|