卡尔曼滤波_卡尔曼滤波算法 - CSDN
卡尔曼滤波 订阅
卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。数据滤波是去除噪声还原真实数据的一种数据处理技术,Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中,估计动态系统的状态。由于它便于计算机编程实现,并能够对现场采集的数据进行实时的更新和处理,Kalman滤波是目前应用最为广泛的滤波方法,在通信,导航,制导与控制等多领域得到了较好的应用。 展开全文
卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。数据滤波是去除噪声还原真实数据的一种数据处理技术,Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中,估计动态系统的状态。由于它便于计算机编程实现,并能够对现场采集的数据进行实时的更新和处理,Kalman滤波是目前应用最为广泛的滤波方法,在通信,导航,制导与控制等多领域得到了较好的应用。
信息
提出时间
1958
表达式
X(k)=A X(k-1)+B U(k)+W(k)
提出者
斯坦利·施密特
应用学科
天文,宇航,气象
中文名
卡尔曼滤波器,Kalman滤波,卡曼滤波
适用领域范围
控制、制导、导航、通讯等现代工程
外文名
KALMAN FILTER
卡尔曼滤波背景
斯坦利·施密特(Stanley Schmidt)首次实 现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时,发现他的方法对于解决阿波罗计划的轨道预测很有用,后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)发表。
收起全文
精华内容
参与话题
  • 详解卡尔曼滤波原理

    万次阅读 多人点赞 2017-03-18 18:21:58
    详解卡尔曼滤波原理 在网上看了不少与卡尔曼滤波相关的博客、论文,要么是只谈理论、缺乏感性,或者有感性认识,缺乏理论推导。能兼顾二者的少之又少,直到我看到了国外的一篇博文,真的惊艳到我了,不得不佩服作者...

    详解卡尔曼滤波原理

      在网上看了不少与卡尔曼滤波相关的博客、论文,要么是只谈理论、缺乏感性,或者有感性认识,缺乏理论推导。能兼顾二者的少之又少,直到我看到了国外的一篇博文,真的惊艳到我了,不得不佩服作者这种细致入微的精神,翻译过来跟大家分享一下,原文链接:http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
      我不得不说说卡尔曼滤波,因为它能做到的事情简直让人惊叹!意外的是很少有软件工程师和科学家对对它有所了解,这让我感到沮丧,因为卡尔曼滤波是一个如此强大的工具,能够在不确定性中融合信息,与此同时,它提取精确信息的能力看起来不可思议。

    什么是卡尔曼滤波?

      你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波,对系统下一步的走向做出有根据的预测,即使伴随着各种干扰,卡尔曼滤波总是能指出真实发生的情况。
      在连续变化的系统中使用卡尔曼滤波是非常理想的,它具有占用内存小的优点(除了前一个状态量外,不需要保留其它历史数据),并且速度很快,很适合应用于实时问题和嵌入式系统。
      在Google上找到的大多数关于实现卡尔曼滤波的数学公式看起来有点晦涩难懂,这个状况有点糟糕。实际上,如果以正确的方式看待它,卡尔曼滤波是非常简单和容易理解的,下面我将用漂亮的图片和色彩清晰的阐述它,你只需要懂一些基本的概率和矩阵的知识就可以了。

    我们能用卡尔曼滤波做什么?

      用玩具举例:你开发了一个可以在树林里到处跑的小机器人,这个机器人需要知道它所在的确切位置才能导航。

    这里写图片描述

      我们可以说机器人有一个状态 这里写图片描述,表示位置和速度:

    这里写图片描述

      注意这个状态只是关于这个系统基本属性的一堆数字,它可以是任何其它的东西。在这个例子中是位置和速度,它也可以是一个容器中液体的总量,汽车发动机的温度,用户手指在触摸板上的位置坐标,或者任何你需要跟踪的信号。
      这个机器人带有GPS,精度大约为10米,还算不错,但是,它需要将自己的位置精确到10米以内。树林里有很多沟壑和悬崖,如果机器人走错了一步,就有可能掉下悬崖,所以只有GPS是不够的。

    这里写图片描述

      或许我们知道一些机器人如何运动的信息:例如,机器人知道发送给电机的指令,知道自己是否在朝一个方向移动并且没有人干预,在下一个状态,机器人很可能朝着相同的方向移动。当然,机器人对自己的运动是一无所知的:它可能受到风吹的影响,轮子方向偏了一点,或者遇到不平的地面而翻倒。所以,轮子转过的长度并不能精确表示机器人实际行走的距离,预测也不是很完美。
      GPS 传感器告诉了我们一些状态信息,我们的预测告诉了我们机器人会怎样运动,但都只是间接的,并且伴随着一些不确定和不准确性。但是,如果使用所有对我们可用的信息,我们能得到一个比任何依据自身估计更好的结果吗?回答当然是YES,这就是卡尔曼滤波的用处。

    卡尔曼滤波是如何看到你的问题的

      下面我们继续以只有位置和速度这两个状态的简单例子做解释。

    这里写图片描述

      我们并不知道实际的位置和速度,它们之间有很多种可能正确的组合,但其中一些的可能性要大于其它部分:

      卡尔曼滤波假设两个变量(位置和速度,在这个例子中)都是随机的,并且服从高斯分布。每个变量都有一个均值 μ,表示随机分布的中心(最可能的状态),以及方差 这里写图片描述,表示不确定性。

      在上图中,位置和速度是不相关的,这意味着由其中一个变量的状态无法推测出另一个变量可能的值。下面的例子更有趣:位置和速度是相关的,观测特定位置的可能性取决于当前的速度:

      这种情况是有可能发生的,例如,我们基于旧的位置来估计新位置。如果速度过高,我们可能已经移动很远了。如果缓慢移动,则距离不会很远。跟踪这种关系是非常重要的,因为它带给我们更多的信息:其中一个测量值告诉了我们其它变量可能的值,这就是卡尔曼滤波的目的,尽可能地在包含不确定性的测量数据中提取更多信息!
      这种相关性用协方差矩阵来表示,简而言之,矩阵中的每个元素 这里写图片描述 表示第 i 个和第 j 个状态变量之间的相关度。(你可能已经猜到协方差矩阵是一个对称矩阵,这意味着可以任意交换 i 和 j)。协方差矩阵通常用“这里写图片描述”来表示,其中的元素则表示为“这里写图片描述 ”。

    gauss_2.png

    使用矩阵来描述问题

      我们基于高斯分布来建立状态变量,所以在时刻 k 需要两个信息:最佳估计 这里写图片描述(即均值,其它地方常用 μ 表示),以及协方差矩阵 这里写图片描述

    这里写图片描述        (1)

      (当然,在这里我们只用到了位置和速度,实际上这个状态可以包含多个变量,代表任何你想表示的信息)。接下来,我们需要根据当前状态k-1 时刻)来预测下一状态k 时刻)。记住,我们并不知道对下一状态的所有预测中哪个是“真实”的,但我们的预测函数并不在乎。它对所有的可能性进行预测,并给出新的高斯分布。

    gauss_7.jpg

      我们可以用矩阵 这里写图片描述 来表示这个预测过程:

    gauss_8.jpg

      它将我们原始估计中的每个点都移动到了一个新的预测位置,如果原始估计是正确的话,这个新的预测位置就是系统下一步会移动到的位置。那我们又如何用矩阵来预测下一个时刻的位置和速度呢?下面用一个基本的运动学公式来表示:

    这里写图片描述

      现在,我们有了一个预测矩阵来表示下一时刻的状态,但是,我们仍然不知道怎么更新协方差矩阵。此时,我们需要引入另一个公式,如果我们将分布中的每个点都乘以矩阵 A,那么它的协方差矩阵 这里写图片描述 会怎样变化呢?很简单,下面给出公式:

    这里写图片描述

      结合方程(4)和(3)得到:

    这里写图片描述

    外部控制量

      我们并没有捕捉到一切信息,可能存在外部因素会对系统进行控制,带来一些与系统自身状态没有相关性的改变。
      以火车的运动状态模型为例,火车司机可能会操纵油门,让火车加速。相同地,在我们机器人这个例子中,导航软件可能会发出一个指令让轮子转向或者停止。如果知道这些额外的信息,我们可以用一个向量这里写图片描述来表示,将它加到我们的预测方程中做修正。
      假设由于油门的设置或控制命令,我们知道了期望的加速度a,根据基本的运动学方程可以得到:

    这里写图片描述

      以矩阵的形式表示就是:

    这里写图片描述

      这里写图片描述称为控制矩阵,这里写图片描述称为控制向量(对于没有外部控制的简单系统来说,这部分可以忽略)。让我们再思考一下,如果我们的预测并不是100%准确的,该怎么办呢?

    外部干扰

      如果这些状态量是基于系统自身的属性或者已知的外部控制作用来变化的,则不会出现什么问题。
      但是,如果存在未知的干扰呢?例如,假设我们跟踪一个四旋翼飞行器,它可能会受到风的干扰,如果我们跟踪一个轮式机器人,轮子可能会打滑,或者路面上的小坡会让它减速。这样的话我们就不能继续对这些状态进行跟踪,如果没有把这些外部干扰考虑在内,我们的预测就会出现偏差。
      在每次预测之后,我们可以添加一些新的不确定性来建立这种与“外界”(即我们没有跟踪的干扰)之间的不确定性模型:

    gauss_9.jpg

      原始估计中的每个状态变量更新到新的状态后,仍然服从高斯分布。我们可以说这里写图片描述的每个状态变量移动到了一个新的服从高斯分布的区域,协方差为这里写图片描述。换句话说就是,我们将这些没有被跟踪的干扰当作协方差为这里写图片描述噪声来处理。

    gauss_10a.jpg

      这产生了具有不同协方差(但是具有相同的均值)的新的高斯分布。

    gauss_10b.jpg

      我们通过简单地添加这里写图片描述得到扩展的协方差,下面给出预测步骤的完整表达式:

    这里写图片描述

      由上式可知,新的最优估计是根据上一最优估计预测得到的,并加上已知外部控制量修正
      而新的不确定性上一不确定预测得到,并加上外部环境的干扰
      好了,我们对系统可能的动向有了一个模糊的估计,用这里写图片描述这里写图片描述来表示。如果再结合传感器的数据会怎样呢?

    用测量值来修正估计值

      我们可能会有多个传感器来测量系统当前的状态,哪个传感器具体测量的是哪个状态变量并不重要,也许一个是测量位置,一个是测量速度,每个传感器间接地告诉了我们一些状态信息。

    gauss_12-624x287.jpg

      注意,传感器读取的数据的单位和尺度有可能与我们要跟踪的状态的单位和尺度不一样,我们用矩阵 这里写图片描述 来表示传感器的数据。

    gauss_13.jpg

      我们可以计算出传感器读数的分布,用之前的表示方法如下式所示:

    这里写图片描述

      卡尔曼滤波的一大优点就是能处理传感器噪声,换句话说,我们的传感器或多或少都有点不可靠,并且原始估计中的每个状态可以和一定范围内的传感器读数对应起来。

    gauss_14.jpg

      从测量到的传感器数据中,我们大致能猜到系统当前处于什么状态。但是由于存在不确定性,某些状态可能比我们得到的读数更接近真实状态。

    gauss_11.jpg

      我们将这种不确定性(例如:传感器噪声)用协方差这里写图片描述表示,该分布的均值就是我们读取到的传感器数据,称之为这里写图片描述
    现在我们有了两个高斯分布,一个是在预测值附近,一个是在传感器读数附近。

    gauss_4.jpg

      我们必须在预测值粉红色)和传感器测量值绿色)之间找到最优解。
      那么,我们最有可能的状态是什么呢?对于任何可能的读数这里写图片描述,有两种情况:(1)传感器的测量值;(2)由前一状态得到的预测值。如果我们想知道这两种情况都可能发生的概率,将这两个高斯分布相乘就可以了。

    gauss_6a.png

      剩下的就是重叠部分了,这个重叠部分的均值就是两个估计最可能的值,也就是给定的所有信息中的最优估计
      瞧!这个重叠的区域看起来像另一个高斯分布。

    gauss_6.png

      如你所见,把两个具有不同均值和方差的高斯分布相乘,你会得到一个新的具有独立均值和方差的高斯分布!下面用公式讲解。

    融合高斯分布

      先以一维高斯分布来分析比较简单点,具有方差 这里写图片描述 和 μ 的高斯曲线可以用下式表示:

    这里写图片描述

      如果把两个服从高斯分布的函数相乘会得到什么呢?

    这里写图片描述

      将式(9)代入到式(10)中(注意重新归一化,使总概率为1)可以得到:

    这里写图片描述

      将式(11)中的两个式子相同的部分用 k 表示:

    这里写图片描述

      下面进一步将式(12)和(13)写成矩阵的形式,如果 Σ 表示高斯分布的协方差,这里写图片描述 表示每个维度的均值,则:

    这里写图片描述

      矩阵这里写图片描述称为卡尔曼增益,下面将会用到。放松!我们快要完成了!

    将所有公式整合起来

      我们有两个高斯分布,预测部分这里写图片描述,和测量部分这里写图片描述,将它们放到式(15)中算出它们之间的重叠部分:

    这里写图片描述

      由式(14)可得卡尔曼增益为:

    这里写图片描述

      将式(16)和式(17)的两边同时左乘矩阵的逆(注意这里写图片描述里面包含了 这里写图片描述 )将其约掉,再将式(16)的第二个等式两边同时右乘矩阵 这里写图片描述 的逆得到以下等式:

    这里写图片描述

      上式给出了完整的更新步骤方程。这里写图片描述就是新的最优估计,我们可以将它和这里写图片描述放到下一个预测更新方程中不断迭代。

    kalflow.png

    总结

      以上所有公式中,你只需要用到式(7)、(18)、(19)。(如果忘了的话,你可以根据式(4)和(15)重新推导一下)
      我们可以用这些公式对任何线性系统建立精确的模型,对于非线性系统来说,我们使用扩展卡尔曼滤波,区别在于EKF多了一个把预测和测量部分进行线性化的过程。

    (ps: 第一次用Markdown,添加图片和公式心累啊,什么时候能直接拖拽就好了~~)

    附Markdown使用技巧:
    1. 改变文本字体、字号与颜色。参考链接:(http://blog.csdn.net/testcs_dn/article/details/45719357/)
    2. 在线公式编辑器,编辑好了右键“复制图片地址”,当作图片来添加。
     链接:(http://private.codecogs.com/latex/eqneditor.php)
    3. 段落首行缩进,按Shift+Space将输入法切换到全角状态,然后敲空格即可,一个空格代表一个汉字的间隔。
    4. 设置图片大小及居中显示。参考链接:(http://blog.csdn.net/soindy/article/details/50427079)
    5. 不懂百度。

    展开全文
  • 十分钟读懂『卡尔曼滤波算法』

    万次阅读 多人点赞 2017-05-04 15:47:11
    我是勤劳的搬运工,转自: 1.http://blog.csdn.net/karen99/article/details/7771743 2.... --------------------------------------------------------------------

    我是勤劳的搬运工,转自:

    1.http://blog.csdn.net/karen99/article/details/7771743

    2.http://blog.csdn.net/tudouniurou/article/details/6277512

    --------------------------------------------------------------------------------

    一、卡尔曼最直白的解释:

            卡尔曼滤波本来是控制系统课上学的,当时就没学明白,也蒙混过关了,以为以后也不用再见到它了,可惜没这么容易,后来学计算机视觉和图像处理,发现用它的地方更多了,没办法的时候只好耐心学习和理解了。一直很想把学习的过程记录一下,让大家少走弯路,可惜总也没时间和机会,直到今天。


              我一直有一个愿望,就是把抽象的理论具体化,用最直白的方式告诉大家--不提一个生涩的词,不写一个数学公式,像讲故事一样先把道理说明白,需要知道细节的同学可以自己去查所有需要知道的一切。因为学习的过程告诉我,最难的其实是最初和这个理论和应用背景亲和的过程--这些理论它究竟是做什么的,又是怎么做到的。可惜我们能看到的关于这些理论的资料大多数都是公式的堆砌并且假定我们明白许多“基本的道理”,其实这些“基本的道理”往往是我们最难想象和超越的。以卡尔曼滤波为例,让我们尝试一种不同的学习方法。


             相信所有学习卡尔曼滤波的同学首先接触的都是状态方程和观测方程,学过控制系统的同学可能不陌生,否则,先被那两个看起来好深奥的公式给吓跑了,关键是还不知道他们究竟是干什么的,什么是状态,什么是观测。。。。。。如果再看到后面的一大串递归推导增益,实在很晕很晕,更糟糕的是还没整明白的时候就已经知道卡尔曼滤波其实已经不够使了,需要extended kalmanfilter 和particle filter了。。。


    其实我们完全不用理会这些公式。先来看看究竟卡尔曼滤波是做什么的,理解了卡尔曼滤波,下面的就顺其自然了。


             用一句最简单的话来说,卡尔曼滤波是来帮助我们做测量的,大家一定不明白测量干嘛搞那么复杂?测量长度拿个尺子比一下,测量温度拿温度表测一下不就完了嘛。的确如此,如果你要测量的东西很容易测准确,没有什么随机干扰,那真的不需要劳驾卡尔曼先生。但在有的时候,我们的测量因为随机干扰,无法准确得到,卡尔曼先生就给我们想了个办法,让我们在干扰为高斯分布的情况下,得到的测量均方误差最小,也就是测量值扰动最小,看起来最平滑。


    还是举例子最容易明白。我最近养了只小兔子,忍不住拿小兔子做个例子嘻嘻。

             每天给兔子拔草,看她香甜地吃啊吃地,就忍不住关心一下她的体重增长情况。那么我们就以小兔子的体重作为研究对象吧。假定我每周做一次观察,我有两个办法可以知道兔子的体重,一个是拿体重计来称:或许你有办法一下子就称准兔子的体重(兽医通常都有这办法),但现在为了体现卡尔曼先生理论的魅力,我们假定你的称实在很糟糕,误差很大,或者兔子太调皮,不能老实呆着,弹簧秤因为小兔子的晃动会产生很大误差。尽管有误差,那也是一个不可失去的渠道来得到兔子的体重。还有一个途径是根据书本上的资料,和兔子的年龄,我可以估计一下我的小兔子应该会多重,我们把用称称出来的叫观察量,用资料估计出来的叫估计值,无论是观察值还是估计值显然都是有误差的,假定误差是高斯分布。现在问题就来了,按照书本上说我的兔子该3公斤重,称出来却只有2.5公斤,我究竟该信哪个呢?如果称足够准,兔子足够乖,卡尔曼先生就没有用武之地了呵呵,再强调一下是我们的现状是兔兔不够乖,称还很烂呵呵。在这样恶劣的情景下,卡尔曼先生告诉我们一个办法,仍然可以估计出八九不离十的兔兔体重,这个办法其实也很直白,就是加权平均,把称称出来的结果也就是观测值和按照书本经验估算出来的结果也就是估计值分别加一个权值,再做平均。当然这两个权值加起来是等于一的。也就是说如果你有0.7分相信称出来的体重,那么就只有0.3分相信书上的估计。说到这里大家一定更着急了,究竟该有几分相信书上的,有几分相信我自己称的呢?都怪我的称不争气,没法让我百分一百信赖它,还要根据书上的数据来做调整。好在卡尔曼先生也体会到了我们的苦恼,告诉我们一个办法来决定这个权值,这个办法其实也很直白,就是根据以往的表现来做决定,这其实听起来挺公平的,你以前表现好,我就相信你多一点,权值也就给的高一点,以前表现不好,我就相信你少一点,权值自然给的低一点。那么什么是表现好表现不好呢,表现好意思就是测量结果稳定,方差很小,表现不好就是估计值或观测值不稳定,方差很大。想象你用称称你的哦兔子,第一次1公斤第二次10公斤,第三次5公斤,你会相信你的称吗,但是如果第一次3公斤第二次3.2公斤,第三次2.8公斤,自然我就相信它多一点,给它一个大的权值了。


            有了这个权值,下面的事情就很好办了。很显然卡尔曼先生是利用多次观察和估计来达到目的的,我们也只能一步一步地调整我们的观察和估计值,来渐渐达到准确的测量,所以整个算法是递归的,需要多次重复调整的。调整的过程也很简单,就是把实测值(称出来的体重)和估计值(书上得来的体重)比较一下,如果估计值比测量值小,那就把估计值加上他们之间的偏差作为新的估计值,当然前面要加个系数,就是我们前面说的加权系数,这个地方我要写个公式,因为很简单就能说明白。


             比如我们的观查值是Z,估计值是X, 那么新的估计值就应该是 Xnew  =  X  + K ( Z-X),从这个公式可以看到,如果X估计小了,那么新的估计值会加上一个量K ( Z-X), 如果估计值大了,大过Z了,那么新的估计值就会减去一个量K ( Z-X),这就保证新的估计值一定比现在的准确,一次一次递归下去就会越来越准却了,当然这里面很有作用的也是这个K,也就是我们前面说的权值,书上都把他叫卡尔曼增益。。。(Xnew  =  X  + K ( Z-X) = X ×(1-K) + KZ ,也就是说估计值X的权值是1-k,而观察值Z的权值是k,究竟k 取多大,全看估计值和观察值以前的表现,也就是他们的方差情况了)


    发现把一个问题讲明白还真不是件容易的事情,谁听明白了我佩服谁,因为我已经把自己讲糊涂了哈

    顺便就把extended kalman filter和particle filter提一下,因为高斯模型有时不适用,于是有了extended kalman filter,而particle filter是用于多个对象的,比如除了兔子我还有只猫,他们的体重有一个联合概率模型,每一个对象就是一个particle。无论是卡尔曼滤波还是particle滤波,都是概率分布传递的过程,卡尔曼传递的是高斯分布,particle filter 传递的是高斯混合分布,每一个峰代表一个动物在我们的例子。


    ------------------------------------------------华丽的分割线------------------------------------------------

    二、卡尔曼滤波之数学建模

    一直在看,一直不懂。 我这人学数学的毛病,就是需要非常细致的知道每个变量的含义,谁变谁不变必须清清楚楚告诉我,否则我就没有那个直觉。 anyway,从这篇文章入手吧:http://www.cs.unc.edu/~welch/kalman/media/pdf/kalman_intro_chinese.pdf


    所谓滤波,实际上是要去掉自己不想要的信号,保留想要的部分。一般来说,是把过程中的噪声去掉。

    卡尔曼滤波的默认假定是,世界充满噪声,任何测量结果都有噪声,状态转移过程会有噪声,你想知道系统的真实值么?玩儿蛋去吧。

    卡尔曼滤波另一个重要假定模型是这样的,一个系统会处在各种不同的状态,并且会在状态之间转化来转化去。但是呢,倒霉的是我们谁也不知道该系统当前到底是在什么状态;但是呢,幸运的是我们可以通过测量的结果猜测到系统当前在一个什么状态。

    那啥叫状态呢?例如系统的速度,加速度,温度,脑残度都算。离散系统的话,我们可以假设一个黑盒,黑盒里有许多颜色的灯(红橙黄绿青蓝紫),同时只能有一个颜色在亮着,ok,哪个灯在亮就是当前状态。

     

    下面就是建模:

    z_t = H*x_t + v_t;                                                                                                                                         (1)

                                 x_t = A*x_(t-1) + B*u_(t-1) + w_(t-1);                                                                                                                      (2)


            初看起这俩式子来,我也头大,不过稍微分析一下也不太难。x_t是t时刻系统所在状态,z_t是所谓观测值,u_t是系统控制变量(已知,但我做的领域一般用不着),w_t , v_t都是噪声。


             那么式子(1)就是想说,观测值和系统状态的关系: 如果你看到种子发芽了,那么它的状态就是刚出生;如果你看到它开始长叶儿了,那状态就叫生长期;如果丫开花了,就叫啥啥期;如果结果了,就叫成熟期;如果蔫儿了,就叫嗝屁期。

     

             哦,对了,个人理解一下,以上公式限定为了线性系统,传说中卡尔曼滤波是线性的,来源就在这里了,谁叫你是矩阵乘向量呢,你要是写成f(x_t),那有可能就是非线性的了。


            那么式子(2)就是说,前一时刻状态和下一时刻状态之间的关系。我在这里卡了好久,总是以为丫是马尔科夫过程,所以就完全无法理解A这个系数是凭啥得来的。其实,就是一个固定的转移方程,该方程完全没有概率问题。


             所以!以上式子中,固定下来的是H, A, B,这三个矩阵千年不变,万年不变,并且是事先设定好的,全都已知。未知的话....你自己编一个模型吧。 那么w_t,v_t 在这里限定为两个高斯白噪声N(0, Q)和N(0, R)。 哦,对,这里要记得,Q,R都tm是协方差矩阵啊,因为,系统状态和观测值都是向量。我对协方差可郁闷可郁闷了。这里提一句,我就完全无法理解协方差想表达什么,为什么俩随机变量独立,协方差一定为0,虽然我也知道怎么推导,但就是不能直观理解之,如果有人知道,还烦请告知。


              那继续扯淡。卡尔曼滤波,本质是想要预测下一步的观测值,或者实时监控一下系统所在状态。但一般在我这个领域,大家还都是在玩儿预测,那咱就从预测角度分析。OK,直觉上,给定上一个位置的状态x_(t-1),式子(2)足够了。但是,回到开始的默认假设,式子(2)得到的结果x^-_t那是各种不准确啊。不准确怎么办?那就去看观测值呗,于是得到观测值z_t,但是观测值也不准确唉,那怎么办?当当当当,卡尔曼告诉我们一个灰常牛B的事情,一个相对准确的系统值具有如下结构:

                                                                                                                        x&_t = x&-_t + K( z_t - H*x_(t-1) ) ;                                                                                                                       (3)

    提一下,这里" & "表示估计值," - "表示是用前面式子算出来的估计值,不带" - "表示是最后的估计值。 (3)这个式子是想说,你不是式子估计和观测值都不准么,那么你把他俩加个权,求个和,那就可能更准确了。啥?你说这个式子不像加权?你把丫拆了,倒腾倒腾不就像了。 所以,最牛B就牛B在了这个“K”,传说中的卡尔曼增益。 这个K怎么得到的?我也不知道。 文章说法是,定义误差 e_t = x_t - x&_t ,P_t为此误差的协方差矩阵,目的是使这个误差协方差矩阵最小化,把(3)代过去,于是折腾来折腾去,再求个导数为0,解得K,这个关键值的算法

                                                                                                                 K = P-_t * H^T * ( H * P-_t * H^T + R) ^(-1);                                                                                                               (4)

    哦,对了,另外注意一点,从此以后,我们就都在估计上做文章了,你只要记得,咱永远看不到真实值就行了,于是我们的式子里不是带"&"就是带"-"。

    那么式子(4)就是在说,你丫这么着就把K求出来了。于是,问题就变成了这个P-_t怎么个求法。

    说到这里,传说中的卡尔曼滤波就算讲完了。神马?你说P-_k还没求呢。是啊,卡尔曼滤波一共就俩需要求的玩意儿,还都tm是迭代求解,一个就是这个P-_t,另一个就是状态x-_t。你随便给个初始值,然后就迭着吧。

    言归正传,我还得给出迭代的公式:

    x-_t = A * x&_(t-1) + B * u_(t-1);                                                                                                                         (5)

    P-_t = A * P_(t-1) * A^T + Q;                                                                                                                            (6)

    大家一定别搞混Q和R谁是哪个公式冒出来的啊。 另外严重关切一下这里"-","&"以及不加的关系。 注意到啥没有?对了,(6)式中等号右边的P_(t-1)不带任何符号,嘿嘿,那自然是还差一个公式啦:

    P_t = (I - K_t * H ) P-_(t-1);                                                                                                                           (7)

    大功告成,以上就是传说中的卡尔曼滤波的迭代求解方法,如果你要预测状态呢,就用式子(5)的结果;如果预测观测值呢,就把式子(5)的结果乘个H;如果是监测状态呢,就用式子(3)的结果。


              至于一切式子中的推导过程,还有为神马是这样求出来的,咕~~(╯﹏╰)b,本人一概不知。泪奔告退。

              最后小注一下,文章指出,如果Q,R是常数,K会收敛,也即P也会收敛到常量。 另外,大家经常诟病卡尔曼滤波都是假定高斯分布,我勒个去,这里的高斯分布到底说谁呢?噪声项?虽然看上去应该是,但我打赌不是。可是其它又都是定值,唉,头大。我本来就是为了理解这句话才来学习卡尔曼滤波的。 还得慢慢学,继续泪奔


    PS:于是,果然,文中提到x_t是一个随机变量,并且在已知z_t的情况下 p(x_t | z_t) 服从N( x&_t, E[(x_t - x&_t)(x_t - x&_t)]),切记切记,这里所说的正态分布,是指已知观测值的情况下,系统状态是一个高斯分布的随机变量。


    展开全文
  • 通俗理解卡尔曼滤波及其算法实现(实例解析)

    万次阅读 多人点赞 2017-05-18 15:13:20
    1.简介(Brief Introduction)在学习卡尔曼滤波器之前,首先看看为什么叫“卡尔曼”。跟其他著名的理论(例如傅立叶变换,泰勒级数等等)一样,卡尔曼也是一个人的名字,而跟他们不同的是,他是个现代人!卡尔曼全名...

    1.简介(Brief Introduction)

    在学习卡尔曼滤波器之前,首先看看为什么叫“卡尔曼”。跟其他著名的理论(例如傅立叶变换,泰勒级数等等)一样,卡尔曼也是一个人的名字,而跟他们不同的是,他是个现代人!

    卡尔曼全名Rudolf Emil Kalman,匈牙利数学家,1930年出生于匈牙利首都布达佩斯。1953,1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器,正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》(线性滤波与预测问题的新方法)。如果对这编论文有兴趣,可以到这里的地址下载: http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf

    卡尔曼滤波器到底是干嘛的?我们来看下wiki上的解释:

    卡尔曼滤波的一个典型实例是从一组有限的,包含噪声的,对物体位置的观察序列(可能有偏差)预测出物体的位置的坐标及速度。在很多工程应用(如雷达、计算机视觉)中都可以找到它的身影。同时,卡尔曼滤波也是控制理论以及控制系统工程中的一个重要课题。例如,对于雷达来说,人们感兴趣的是其能够跟踪目标。但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响,得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计(滤波),也可以是对于将来位置的估计(预测),也可以是对过去位置的估计(插值或平滑)。


    斯坦利.施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时,发现他的方法对于解决阿波罗计划的轨道预测很有用,后来阿波罗飞船的导航电脑便使用了这种滤波器。 关于这种滤波器的论文由Swerling (1958)、Kalman (1960)与 Kalman and Bucy (1961)发表。


    目前,卡尔曼滤波已经有很多不同的实现.卡尔曼最初提出的形式现在一般称为简单卡尔曼滤波器。除此以外,还有施密特扩展滤波器、信息滤波器以及很多Bierman, Thornton 开发的平方根滤波器的变种。也许最常见的卡尔曼滤波器是锁相环,它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。

    简单来说,卡尔曼滤波器是一个“optimal recursive data processing algorithm(最优化自回归数据处理算法)”。对于解决很大部分的问题,他是最优,效率最高甚至是最有用的。他的广泛应用已经超过30年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等。

    2.卡尔曼滤波器的介绍(Introduction to the Kalman Filter)


    为了可以更加容易的理解卡尔曼滤波器,首先应用形象的描述方法来讲解,然后我们结合其核心的5条公式进行进一步的说明和探索。结合现代的计算机,其实卡尔曼的程序相当的简单,只要你理解了他的那5条公式。
    在介绍他的5条公式之前,先让我们来根据下面的例子做个直观的解释。


    假设我们要研究的对象是一个房间的温度。根据你的经验判断,这个房间的温度是恒定的,也就是下一分钟的温度等于现在这一分钟的温度(假设我们用一分钟来做时间单位)。假设你对你的经验不是100%的相信,可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声(White Gaussian Noise),也就是这些偏差跟前后时间是没有关系的而且符合高斯分配(Gaussian Distribution)。另外,我们在房间里放一个温度计,但是这个温度计也不准确的,测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。


    好了,现在对于某一分钟我们有两个有关于该房间的温度值:你根据经验的预测值(系统的预测值)和温度计的值(测量值)。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。
    假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值,来预测k时刻的温度。因为你相信温度是恒定的,所以你会得到k时刻的温度预测值是跟k-1时刻一样的,假设是23度,同时该值的高斯噪声的偏差是5度(5是这样得到的:如果k-1时刻估算出的最优温度值的偏差是3,你对自己预测的不确定度是4度,他们平方相加再开方,就是5)。然后,你从温度计那里得到了k时刻的温度值,假设是25度,同时该值的偏差是4度。


    由于我们用于估算k时刻的实际温度有两个温度值,分别是23度和25度。究竟实际温度是多少呢?相信自己还是相信温度计呢?究竟相信谁多一点,我们可以用他们的covariance来判断。因为Kg^2=5^2/(5^2+4^2),所以Kg=0.78,我们可以估算出k时刻的实际温度值是:23+0.78*(25-23)=24.56度。可以看出,因为温度计的covariance比较小(比较相信温度计),所以估算出的最优温度值偏向温度计的值。


    现在我们已经得到k时刻的最优温度值了,下一步就是要进入k+1时刻,进行新的最优估算。到现在为止,好像还没看到什么自回归的东西出现。对了,在进入k+1时刻之前,我们还要算出k时刻那个最优值(24.56度)的偏差。算法如下:((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差,得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差(对应于上面的3)。


    就是这样,卡尔曼滤波器就不断的把covariance递归,从而估算出最优的温度值。他运行的很快,而且它只保留了上一时刻的covariance。上面的Kg,就是卡尔曼增益(Kalman Gain)。他可以随不同的时刻而改变他自己的值,是不是很神奇!
    下面就要言归正传,讨论真正工程系统上的卡尔曼。

    3. 卡尔曼滤波器算法(The Kalman Filter Algorithm)


    在这一部分,我们就来描述源于Dr Kalman 的卡尔曼滤波器。下面的描述,会涉及一些基本的概念知识,包括概率(Probability),随即变量(Random Variable),高斯或正态分配(Gaussian Distribution)还有State-space Model等等。但对于卡尔曼滤波器的详细证明,这里不能一一描述。

    首先,我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程(Linear Stochastic Difference equation)来描述,我们结合下面PPT截图进行说明:

    上两式子中,x(k)是k时刻的系统状态,u(k)是k时刻对系统的控制量。A和B是系统参数,对于多模型系统,他们为矩阵。y(k)是k时刻的测量值,H是测量系统的参数,对于多测量系统,H为矩阵。q(k)和r(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise),他们的covariance分别是Q,R(这里我们假设他们不随系统状态变化而变化)。

    对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器。先给出KF算法的流程和五个核心更新方程如下:

    KF算法

    五个更新方程为:

    编写公式不方便,所以写成了PDF然后做了截图粘在了下面,下面就上面的例子和五个核心的公式对Kalman算法进行下说明:

    就这样,算法就可以自回归的运算下去。

    看到这聪明的同学可能已经看出来了,问道卡尔曼增益为什么会是第三步中那样求,现在只大致说一下原理,具体推到比较复杂,有兴趣的同学可以参考这文献去推一推。


    还记得前面我们说的误差协方差矩阵$P_k$么,即求第k次最优温度的误差协方差矩阵,对应于上例中的3和2.35....这些值。看下面PPT,我们最小化P即可得到卡尔曼增益K,对应上例求解K只最小化最优温度值的偏差,即最小化P(K):

    我们由第四步可以看出,k时刻系统的最优温度值=k-1时刻状态估计值(由上一状态的最优温度值加上过程误差)+带卡尔曼增益权值项的偏差。如果观测误差远远大于估计误差,那么K就很小,k时刻的预测值约等于k时刻的状态估计值,如果对i时刻的状态估计值误差远远大于观测误差,此时相应的q较大,K较大,i时刻的状态估计值更倾向于观察的数据。

    卡尔曼滤波器的原理基本描述就完成了,希望能帮助大家理解这这5个公式,其算法可以很容易的用计算机的程序实现。下面,我会用程序举一个实际运行的例子。

    4. 简单例子(A Simple Example)
    这里我们结合第二第三节,举一个非常简单的例子来说明卡尔曼滤波器的工作过程。所举的例子是进一步描述第二节的例子,而且还会配以程序模拟结果。


    根第二节的描述,把房间看成一个系统,然后对这个系统建模。当然,我们见的模型不需要非常地精确。我们所知道的这个房间的温度是跟前一时刻的温度相同的,所以A=1。没有控制量,所以u(k)=0。因此得出:
    x(k|k-1)=x(k-1|k-1) ……… (6)
    式子(2)可以改成:
    P(k|k-1)=P(k-1|k-1) +Q ……… (7)


    因为测量的值是温度计的,跟温度直接对应,所以H=1。式子3,4,5可以改成以下:
    X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)
    Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)
    P(k|k)=(1-Kg(k))P(k|k-1) ……… (10)


    现在我们模拟一组测量值作为输入。假设房间的真实温度为25度,我模拟了200个测量值,这些测量值的平均值为25度,但是加入了标准偏差为几度的高斯白噪声(在图中为蓝线)。


    为了令卡尔曼滤波器开始工作,我们需要告诉卡尔曼两个零时刻的初始值,是X(0|0)和P(0|0)。他们的值不用太在意,随便给一个就可以了,因为随着卡尔曼的工作,X会逐渐的收敛。但是对于P,一般不要取0,因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的,从而使算法不能收敛。我选了X(0|0)=1度,P(0|0)=10。

    该系统的真实温度为25度,图中用黑线表示。图中红线是卡尔曼滤波器输出的最优化结果(该结果在算法中设置了Q=1e-6,R=1e-1)。

    附matlab下面的kalman滤波程序:

    1. clear    
    2. N=200;    
    3. w(1)=0;                     %w为过程噪声  
    4. w=randn(1,N)    
    5. x(1)=25;    
    6. a=1;                        %a为方程中A(k)  
    7. for k=2:N;    
    8. x(k)=a*x(k-1)+w(k-1);    
    9. end    
    10. V=randn(1,N);               %V为观察噪声  
    11. q1=std(V);    
    12. Rvv=q1.^2;    
    13. q2=std(x);    
    14. Rxx=q2.^2;     
    15. q3=std(w);    
    16. Rww=q3.^2;    
    17. c=0.2;                      %c为方程中H(k)  
    18. Y=c*x+V;                    %Y为观察值  
    19. p(1)=0;    
    20. s(1)=0;    
    21. for t=2:N;    
    22. p1(t)=a.^2*p(t-1)+Rww;     %p1为方程中p'  
    23. b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);    
    24. s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));    
    25. p(t)=p1(t)-c*b(t)*p1(t);    
    26. end    
    27. t=1:N;    
    28. plot(t,s,'r',t,Y,'g',t,x,'b'); 

    更为详细的过程可参考有关的资料。

    文章参考了:

    1 博文http://hi.baidu.com/irvkqscjezbrtwq/item/4ad3bb018b8c7e37a3332a07

    2 自动化所董秋雷上课课件

    3 《学习Opencv》 于仕琪 P384 kalman滤波器部分

    4 如果做视频跟踪具体参数选择可参考《数字视频处理》黎洪松 P102-106

    5 如果想探索其具体推导过程可参考《现代信号处理》 张贤达 P177-188

    展开全文
  • 卡尔曼滤波详解

    万次阅读 多人点赞 2020-09-16 11:18:26
    主要讲解基本的卡尔曼滤波算法,有时候也说是离散或者线性卡尔曼滤波。 首先来看一个数学公式,这部分仅仅是给定一个思路,和最后实际算法无关。目前考虑到要估计当前系统的状态,而且有两个已知量,一个上一个状态...

    这篇主要介绍卡尔曼滤波公式详细推导,使用示例参考卡尔曼滤波示例

    Kalman Filter

    简单介绍

    主要讲解基本的卡尔曼滤波算法,有时候也说是离散或者线性卡尔曼滤波。

    首先来看一个数学公式,这部分仅仅是给定一个思路,和最后实际算法无关。目前考虑到要估计当前系统的状态,而且有两个已知量,一个上一个状态的估计值以及当前状态的测量值,这两个都有一定的噪声,需要做的就是把这两个结合起来,很简单的思路就是按照比例相加得到当前状态的估计值:
    X^k=KkZk+(1Kk)X^k1 \hat{X}_k = K_k \cdot Z_k + (1 - K_k) \cdot \hat{X}_{k-1}

    kk 表示离散的状态量,可以把它简单的理解为离散的时间间隔。k=1 表示1ms,k=2 表示2ms;

    X^k\hat{X}_k 是对当前状态的估计值,希望利用上面的公式对每一个 k 都能得到一个较为准确的 X 的值;

    ZkZ_k 是对当前状态的测量值,当然这个值并不是绝对准确的,会有一定的误差噪声(如果绝对准确,直接用就可以了,也就没必要搞这个卡尔曼滤波算法了);

    X^k1\hat{X}_{k-1} 是对上一状态的估计值,利用这个以及测量值对当前状态进行估计;

    KkK_k 是卡尔曼增益(kalman gain),在这里唯一未知的就是这个值,也是需要去求的值。当然可以直接设置值为0.5,但是这样比较暴力。最好的方式就是根据每一时刻的状态求一个当前状态最好的增益值,这样的话更好利用以前状态的估计值以及当前测量值来估计一个最优的当前值。

    后面卡尔曼滤波算法就是按照上面思路利用上一状态以及测量值去估计当前状态,只不过模型要更加复杂。

    基本模型

    卡尔曼滤波的状态方程,利用线性随机差分方程(Linear Stochastic Difference equation)利用上一个系统状态估计当前系统状态(这里假设上一状态与下一一状态有某种线性关系,比如恒温环境的温度,匀速运动的速度等,但是因为现实环境的复杂,这种线性关系不是完全平滑的,也就是会有一些扰动):
    xk=Axk1+Buk1+w x_k = Ax_{k-1} + Bu_{k-1}+w
    使用时一般忽略 uu 控制输入,得到:
    xk=Axk1+w(1.1) x_k = Ax_{k-1} + w \qquad {(1.1)}
    加上对于当前状态的测量方程(简单来说就是测量值和状态值的线性函数):
    zk=Hxk+v(1.2) z_k = Hx_k + v \qquad {(1.2)}

    k1k-1kk 分别表示上一状态和当前状态;

    xRnx \in R^n 表示要估计的状态;

    ARn×nA \in R^{n \times n} 表示上一状态到当前状态的转换矩阵;

    uRlu \in R^l 表示可选的控制输入,一般在实际使用中忽略;

    BRn×lB \in R^{n \times l} 表示控制输入到当前状态的转换矩阵;

    zRmz \in R^m 表示测量值;

    HRm×nH \in R^{m \times n} 表示当前状态到测量的转换矩阵;

    wRnw \in R^n 表示过程噪声,主要是从上一状态进入到当前状态时,会有许多外界因素的干扰;

    vRmv \in R^m 表示测量噪声,主要是任何测量仪器都会有一定的误差;

    在上面转换矩阵 AA BB HH ww vv 是随着状态变化的,在这里没有添加下标,假设是不变的。

    上面提到的过程噪声 ww 和测量噪声 vv 假设是相互独立的(之间没有关系,无相互影响),且是高斯白噪声,意思是这些噪声在离散的状态上是没有关系的(互相独立的, 每个时刻的噪声都是独立的)且 服从高斯分布:
    p(w)N(0,Q)(1.3)p(v)N(0,R)(1.4)Q=wwTR=vvTE(w)=0E(v)=0 \begin{aligned} p(w) & \sim N(0, Q) \qquad {(1.3)} \\ p(v) & \sim N(0, R) \qquad {(1.4)} \\ Q & = ww^T \\ R & = vv^T \\ E(w) & = 0 \\ E(v) & = 0 \end{aligned}

    QRn×nQ \in R^{n \times n} 表示过程噪声 ww 的协方差矩阵,表示 ww 向量元素之间的相关关系;

    RRm×mR \in R^{m \times m} 表示测量噪声 vv 的协方差矩阵,表示 vv 向量元素之间的相关关系;

    在上面协方差矩阵 QQ RR 是随着状态变化的,在这里假设是不变的。

    推导过程

    上面是给出了卡尔曼滤波的整体思路和基本模型,在这里一步步分解来看卡尔曼滤波的推导过程。首先确定了卡尔曼滤波的基本模型,那么下一步是怎样根据这个模型来不断求取后面的状态。

    首先定义几个变量:

    x^kˉ\bar{\hat{x}_k} 加了一个负号上标,表示仅仅利用过程先验知识求出的当前状态的先验状态估计(a priori state estimate),不考虑过程噪声的情况下;

    x^k\hat{x}_k 表示利用测量值 zkz_k 求出的当前状态的后验状态估计(a posteriori state estimate),也是最终求得的状态;

    需要注意的是,卡尔曼滤波的最终目标是求 x^k\hat{x}_k ,这是对当前状态的最优估计。

    根据上面的定义忽略过程噪声可以得到:
    x^kˉ=Ax^k1+Buk1 \bar{\hat{x}_k} = A\hat{x}_{k-1} + Bu_{k-1}
    通常都是忽略 uu 这个可选的控制输入(以后加入该变量的话也是非常方便的):
    x^kˉ=Ax^k1(2.1) \bar{\hat{x}_k} = A\hat{x}_{k-1} \qquad {(2.1)}
    然后同理忽略测量噪声可以得到:
    zkˉ=Hx^kˉ \begin{aligned} \bar{z_k} & = H\bar{\hat{x}_k} \end{aligned}

    zkˉ\bar{z_k} 表示忽略测量噪声根据当前先验状态得到的无噪声测量值。

    那么很简单的可以用实际得到的测量值减去无噪声由上一状态预测的测量值:
    zk˙=zkzkˉ=zkHx^kˉ \begin{aligned} \dot{z_k} & = z_k - \bar{z_k} \\ & = z_k - H\bar{\hat{x}_k} \end{aligned}

    zk˙\dot{z_k} 表示实际测量值(包含了上面提到的各种干扰即过程噪声以及测量噪声)减去无噪声测量值,一般被叫做measurement innovation 或者是 residual,表示预测的测量值和实际测量值的差异。实际得到是过程噪声和测量噪声对当前测量值的影响(当然也会影响当前状态),也就是包含了 wwvv 的情况。

    现在得到了一个无噪声的状态的估计 x^kˉ\bar{\hat{x}_k},以及一个噪声变量 zk˙\dot{z_k}。根据经验来说把这两个值按照一定的方式整合起来就可以得到后验状态估计。卡尔曼滤波中采用按照比例组合的方式(其实对于下面的公式在数学上有严格的证明过程,但是仅仅是使用卡尔曼滤波就不去深究了):
    x^k=x^kˉ+Kkzk˙=x^kˉ+Kk(zkHx^kˉ)(2.2) \begin{aligned} \hat{x}_k & = \bar{\hat{x}_k} + K_k\dot{z_k} \\ & = \bar{\hat{x}_k} + K_k(z_k - H\bar{\hat{x}_k}) \qquad {(2.2)} \end{aligned}

    KkRn×mK_k \in R^{n \times m} 表示卡尔曼增益(gain)或者均化系数(blending factor)。

    到这里,有了上面的公式(2.2),为了得到 x^k\hat{x}_k,其他参数都是已知的,目前需要把每个状态的 KkK_k 求出来,这里定义先验和后验估计误差:
    ekˉxkx^kˉekxkx^k \bar{e_k} \equiv x_k - \bar{\hat{x}_k} \\ e_k \equiv x_k - \hat{x}_k
    对应的误差协方差矩阵为:
    Pkˉ=E[ekˉeˉkT]Pk=E[ekekT] \bar{P_k} = E[\bar{e_k}\bar{e}_{k}^T] \\ P_k = E[e_ke_{k}^T]
    然后为了使后验状态估计 x^k\hat{x}_k 最优,那么目标就是最小化后验状态估计的误差协方差矩阵 PkP_k 值。

    后验误差

    根据公式(1.1),(2.2),(1.2),(2.1)可以得到:
    ek=xkx^k=xk(x^kˉ+Kk(zkHx^kˉ))=xk(x^kˉ+Kk((Hxk+v)Hx^kˉ))=xkx^kˉKkHxkKkv+KkHx^kˉ=(IKkH)(xkx^kˉ)Kkv=(IKkH)ekˉKkv \begin{aligned} e_k & = x_k - \hat{x}_k \\ & = x_k - (\bar{\hat{x}_k} + K_k(z_k - H\bar{\hat{x}_k})) \\ & = x_k - (\bar{\hat{x}_k} + K_k((Hx_k + v) - H\bar{\hat{x}_k})) \\ & = x_k - \bar{\hat{x}_k} - K_kHx_k - K_kv + K_kH\bar{\hat{x}_k} \\ & = (I - K_kH)(x_k-\bar{\hat{x}_k}) - K_kv \\ & = (I - K_kH)\bar{e_k} - K_kv \end{aligned}
    所以带入展开可得:
    ekekT=[(IKkH)ekˉKkv][(IKkH)ekˉKkv]T=(IKkH)ekˉekˉT(IKkH)TKkvekˉT(IKkH)T(IKkH)ekˉvTKkT+KkvvTKkT \begin{aligned} e_ke_{k}^T & = \left[ (I - K_kH)\bar{e_k} - K_kv \right]\left[ (I - K_kH)\bar{e_k} - K_kv \right]^T \\ & = (I - K_kH)\bar{e_k}\bar{e_k}^T(I - K_kH)^T - K_kv\bar{e_k}^T(I - K_kH)^T - (I - K_kH)\bar{e_k}v^TK_k^T + K_kvv^TK_k^T \end{aligned}
    ekˉ\bar{e_k}z1,,zk1z_{1} , \cdots , z_{k - 1} 的线性函数,和当前 zkz_k 无关,所以:

    E(ekˉvT)=0E(vekˉT)=0 E(\bar{e_k}v^T) = 0 \\ E(v\bar{e_k}^T) = 0

    带入整理得到:
    Pk=E[ekekT]=(IKkH)Pkˉ(IKkH)T+KkRKkT=PkˉKkHPkˉPkˉHTKkT+KkHPkˉHTKkT+KkRKkT \begin{aligned} P_k = E[e_ke_{k}^T] & = (I-K_kH)\bar{P_k}(I - K_kH)^T + K_kRK_k^T \\ & = \bar{P_k} - K_kH\bar{P_k} - \bar{P_k}H^TK_k^T + K_kH\bar{P_k}H^TK_k^T + K_kRK_k^T \end{aligned}
    合并 KkK_k 项目:
    Pk=PkˉPkˉHT(HPkˉHT+R)1HPkˉ+[KkPkˉHT(HPkˉHT+R)1](HPkˉHT+R)[KkPkˉHT(HPkˉHT+R)1]T \begin{aligned} P_k & = \bar{P_k} - \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1}H\bar{P_k} \\ & + \left[ K_k - \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1}\right ](H\bar{P_k}H^T + R)\left[K_k - \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1} \right ]^T \end{aligned}

    具体合并的过程其实利用最基本的矩阵运算就可以解决,这里主要是矩阵运算可能不那么直观,如果把它想象成正常的以 KkK_k
    为未知数的一元二次方程,可能就很好理解了。首先我们目标是合并 KkK_k 项,先考虑二次项部分: KkHPkˉHTKkT+KkRKkT=Kk(HPkˉHT+R)KkT \begin{aligned} K_kH\bar{P_k}H^TK_k^T + K_kRK_k^T & =K_k(H\bar{P_k}H^T + R)K_k^T \end{aligned} 然后把剩余的两个一次项添加上面合并好的二次项里面,为了抵消上面中间项 HPkˉHT+RH\bar{P_k}H^T + R,所以在添加的时候会额外乘以一个 (HPkˉHT+R)1(H\bar{P_k}H^T + R)^{-1},然后直接就合并好了所有的 KkK_k 项: [KkPkˉHT(HPkˉHT+R)1](HPkˉHT+R)[KkPkˉHT(HPkˉHT+R)1]T \left[ K_k - \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1}\right ](H\bar{P_k}H^T + R)\left[K_k - \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1} \right ]^T 当然这里还有个问题是上面合并项里面会多出来一下之前不存在的项,把上面合并好的公式展开然后减去最开始的 KkK_k
    项,得到了不包含 KkK_k 项的一些补偿项,这样把补偿项添加回去就完成了。

    上面的式子中前两项不包含 KkK_k 因子,所以如果要最小化 PkP_k,只需:
    KkPkˉHT(HPkˉHT+R)1=0 K_k - \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1} = 0
    所以得到:
    Kk=PkˉHT(HPkˉHT+R)1(2.5) K_k = \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1} \qquad {(2.5)}
    然后可以得到:
    Pk=(IKkH)Pkˉ(2.4) P_k = (I-K_kH)\bar{P_k} \qquad {(2.4)}

    先验误差

    带入上面的公式(1.1),(2.1)可以得到:
    ekˉ=xkx^kˉ=(Axk1+w)(Ax^k1)=A(xk1xk1^)+w=Aek1+w \begin{aligned} \bar{e_k} & = x_k - \bar{\hat{x}_k} \\ & = (Ax_{k-1} + w) - (A\hat{x}_{k-1}) \\ & = A(x_{k-1} - \hat{x_{k-1}}) + w \\ & = Ae_{k-1} + w \end{aligned}
    所以带入展开可得:
    ekˉeˉkT=(Aek1+w)(Aek1+w)T=Aek1ek1TAT+wwT+wek1TAT+Aek1wT \begin{aligned} \bar{e_k}\bar{e}_{k}^T & = (Ae_{k-1} + w)(Ae_{k-1} + w)^T \\ & = Ae_{k-1}e_{k-1}^TA^T + ww^T + we_{k-1}^TA^T + Ae_{k-1}w^T \end{aligned}

    又因为:
    E(ek1wT)=0E(wek1T)=0 E(e_{k-1}w^T) = 0 \\ E(we_{k-1}^T) = 0

    然后可以得到:
    Pkˉ=E[ekˉeˉkT]=APk1AT+Q(2.3) \bar{P_k} = E[\bar{e_k}\bar{e}_{k}^T] = AP_{k-1}A^T + Q \qquad {(2.3)}

    基本分析

    从上面的公式可以看出来,如果过程误差 Pkˉ=0\bar{P_k} = 0 ,那么 Kk=0K_k = 0 ,可以 得到 x^k=x^kˉ=Ax^k1\hat{x}_k = \bar{\hat{x}_k} = A\hat{x}_{k-1} ,完全相信以前估计的值,这样就是为啥不能设置 P0=0P_0 = 0 。当然给 QQ 赋值可能能解决这个问题,而且不能设置太小,这样手动引入较大的过程误差,估计结果很不平滑,后面示例3也会讲解。对于RR 的变化对结果的作用后面的示例会介绍,同理 RR 设置太大也会导致这样的结果。在这种情况下,如果初始时设置的不正确,整个估计都是错误的。

    基本公式

    最后得到了上面的上面(2.1)-(2.5)五个方程式,是经典的卡尔曼滤波的五个基本公式,利用这五个公式就可以完成卡尔曼滤波的求解过程。根据迭代过程分成两个方程集合,一个是time update方程,一个是measurement update方程,如下:

    Time Update Measurement Update
    x^kˉ=Ax^k1\bar{\hat{x}_k} = A\hat{x}_{k-1} Kk=PkˉHT(HPkˉHT+R)1K_k = \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1}
    Pkˉ=APk1AT+Q\bar{P_k} = AP_{k-1}A^T + Q x^k=x^kˉ+Kk(zkHx^kˉ)\hat{x}_k = \bar{\hat{x}_k} + K_k(z_k - H\bar{\hat{x}_k})
    Pk=(IKkH)PkˉP_k = (I - K_kH)\bar{P_k}

    迭代过程

    得到上面的基本公式以后,就可以计算计算后面的状态,但是公式中有一些参数是未知的,需要定义,如下:

    uu 变量一般来说是被忽略的;

    AABBHH 上面提到对于每个状态 kk 都是相同的,同时大部分情况下都是单位矩阵(复杂的计算这里不考虑,由具体应用决定);

    RR 一般来说是相对简单可以求到的,就是仪器的噪声协方差;

    QQ 就很难直接的获得,也没有特定的方法,一般是系统过程比较稳定,也就是没有过程噪声;

    zz 所有状态的的测量值都是由仪器测量得到的是已知的;

    对于RRQQ 的取值来说不是特别重要,虽然可能这两个参数估计的不是很准确(不可能符合纯粹的高斯或者正太分布),但是实际情况中卡尔曼滤波算法也可以得到整的比较准确态估计,但是需要记住的是:

    The better you estimate the noise parameters, the better estimates you get.

    那么这样只需要传入基本的 x^0\hat{x}_0P0P_0 就可以了,一般来说需要进行指定。对于 x^0\hat{x}_0 来说不需要什么技巧,因为随着卡尔曼滤波算法的运行,xx 会逐渐的收敛。但是对于 P0P_0 ,一般不要取0,因为这样可能会令卡尔曼完全相信你给定的 x^0\hat{x}_0 是系统最优的,从而使算法不能收敛。

    算法迭代过程如下图(其中 uu 控制输入一般是忽略的):

    Kalman Filter Operation

    参考

    1. Kalman Filter For Dummies
    2. Greg Welch, Gary Bishop, “An Introduction to the Kalman Filter”, University of North Carolina at Chapel Hill Department of Computer Science, 2001
    3. 一个应用实例详解卡尔曼滤波及其算法实现
    4. 卡尔曼滤波算法–核心公式推导导论
    展开全文
  • 再谈卡尔曼滤波--五大公式

    万次阅读 多人点赞 2017-06-24 13:28:18
    这是另一片写卡尔曼滤波的文章,亮点在与总结的卡尔曼滤波的五个公式,可通过上一篇理解卡尔曼滤波的推导原理,本篇用来理解卡尔曼滤波的计算实现 1.简介(Brief Introduction) 在学习卡尔曼滤波器之前,首先看看...
  • 卡尔曼滤波

    千次阅读 2018-08-28 12:39:00
    卡尔曼滤波 本文参考文章《Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation》,使用火车运动的例子进行卡尔曼滤波的推导,并用Python实现。 1. 简介 卡拉曼滤波广泛应用于...
  • 卡尔曼滤波应用及其matlab实现

    万次阅读 多人点赞 2019-12-02 14:46:42
    线性卡尔曼滤波 卡尔曼滤波在温度测量中的应用 X(k)=A*X(k-1)+T*W(k-1) Z(k)=H*X(k)+V(k) 房间温度在25摄氏度左右,测量误差为正负0.5摄氏度,方差0.25,R=0.25。Q=0.01,A=1,T=1,H=1。 假定快时刻...
  • 卡尔曼滤波系列——(一)标准卡尔曼滤波

    万次阅读 多人点赞 2020-09-24 12:41:50
    卡尔曼滤波(Kalman Filter)是一种利用线性系统状态方程,利用对系统的观测数据,对系统状态进行最优估计的算法。由于观测数据受到系统中的噪声和干扰的影响,所以系统状态的估计过程也可看作是滤波过程。应用场景...
  • 无迹卡尔曼滤波(UKF)详解

    万次阅读 2018-11-23 18:21:12
  • 卡尔曼滤波数据融合算法

    万次阅读 多人点赞 2017-12-06 16:46:24
    卡尔曼滤波数据融合算法 小狼@http://blog.csdn.net/xiaolangyangyang
  • 学习卡尔曼滤波看了4天的文章,硬是没看懂.后来找到了下面的文章一下就看懂了. 我对卡尔曼滤波的理解, 我认为,卡尔曼滤波就是把统计学应用到了滤波算法上.  算法的核心思想是,根据当前的仪器测量值,和上一刻的预测量...
  • 学习卡尔曼滤波看了4天的文章,硬是没看懂.后来找到了下面的文章一下就看懂了. 我对卡尔曼滤波的理解, 我认为,卡尔曼滤波就是把统计学应用到了滤波算法上.  算法的核心思想是,根据当前的仪器”测量值” ...
  • mpu6050卡尔曼滤波程序及分析

    万次阅读 2015-09-25 09:10:17
    最近在学习卡尔曼滤波算法,算法 首先静止传感器,先测量100次,求平均值,求出偏差Ax_offset Az_offset Gz_offset.以后每次测量值 都减去这一偏差。然后通过加速度测得的Ax,Az通过 atant(Ax,Az)计算Accel_x 即...
  • 卡尔曼滤波的应用领域与适用范围

    千次阅读 2017-02-14 16:30:41
    KF的应用领域:目标跟踪、故障诊断、计量经济学、惯导系统…… KF的适用范围:准确已知的线性系统和量测方程;系统噪声和量测噪声为互不相关的零均值高斯白噪声。
  • 现在做一个无线的定位系统,求一个Java实现的卡尔曼滤波算法
  • 请问在使用卡尔曼滤波器时,Q和P矩阵初始值如何确定? 例如,我先生成小车的运动轨迹再用卡尔曼滤波器进行预测时。假设观测噪声为零均值、标准差为50m的高斯白噪声;加速度零均值、标准差为2米每秒方的高斯随机变量...
  • 学习卡尔曼滤波看了4天的文章,硬是没看懂.后来找到了下面的文章一下就看懂了. 我对卡尔曼滤波的理解, 我认为,卡尔曼滤波就是把统计学应用到了滤波算法上.  算法的核心思想是,根据当前的仪器"测量值" ...
  • 卡尔曼滤波算法目标跟踪

    千次阅读 2019-02-22 21:59:44
    卡尔曼滤波算法是一种基于最小均方误差的最优线性递归滤波方法,以状态方程和观测方程为基础,运用递归方法来预测线性系统变化。 状态方程和观测方程如下: X(k)是k时刻的状态向量,A是状态系统矩阵,是状态系统...
  • 卡尔曼滤波的五个公式

    千次阅读 2019-01-02 10:17:33
    预测公式: 1.Xkp=AXk-1+Buk+wk 2.Pkp=APk-1AT+Qk A:状态转移矩阵 B:控制矩阵 Wk:预测噪声 Qk:状态转移噪声 --------------------- 状态更新: 3.K=PkpHTHPkpHT+R   ... ...
  • 自适应卡尔曼滤波与H∞滤波

    万次阅读 2014-11-09 10:46:35
    对于离散化后的系统状态方程
1 2 3 4 5 ... 20
收藏数 9,380
精华内容 3,752
关键字:

卡尔曼滤波