精华内容
下载资源
问答
  • 详解卡尔曼滤波原理

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

    详解卡尔曼滤波原理

      在网上看了不少与卡尔曼滤波相关的博客、论文,要么是只谈理论、缺乏感性,或者有感性认识,缺乏理论推导。能兼顾二者的少之又少,直到我看到了国外的一篇博文,真的惊艳到我了,不得不佩服作者这种细致入微的精神,翻译过来跟大家分享一下,原文链接: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. 不懂百度。

    展开全文
  • 卡尔曼滤波原理

    2018-11-24 15:22:57
    卡尔曼滤波原理,最深入浅出的理解卡尔曼滤波器。
  • 卡尔曼滤波原理详解及系统模型建立(simulink)

    万次阅读 多人点赞 2019-12-06 11:27:56
    卡尔曼滤波原理详解及系统模型建立(simulink) 卡尔曼滤波器 卡尔曼滤波器是在上个世界五六十年代的时候提出的,到今天已经有六十年左右的时间,但卡尔曼滤波算法不管在控制、制导、导航或者通讯方面对数据的预测...

    卡尔曼滤波原理详解及系统模型建立(simulink)

    卡尔曼滤波器

    卡尔曼滤波器是在上个世纪五六十年代的时候提出的,到今天已经有六十年左右的时间,但卡尔曼滤波算法不管在控制、制导、导航或者通讯方面对数据的预测能力依然处在一个不可撼动的位置上,可是很多人对于其算法内部的工作原理究竟是怎么运作的依然不理解,所以在工程上很多人都只是把卡尔曼滤波当成是一种“黑箱”预测算法,并不清楚内部原理。但实际上没有任何算法是“黑箱”,只是算法内部的运行规律并不直观,所以让人很难理解,现在也有很多对卡尔曼滤波的解释,但是我这篇文章里希望从原理入手,尽可能定性地对卡尔曼滤波的每一步都做出更加通俗的解释,最后对卡尔曼滤波的系统过程建立相对应的模型,对其进行各种响应的测试,这样也能够更深入地理解卡尔曼滤波。

    原理

    首先我们要明白的就是,卡尔曼滤波不是传统意义上我们理解的滤波,这是一种算法,在变化的数据中去除噪声对系统未来输出做出预测的算法,是基于概率统计原理的预测算法,是一整个系统,是系统就有输入和输出,所以第一步,我们要明确卡尔曼滤波的输入输出到底是在干啥玩意儿。

    很简单看下面这个图:
    卡尔曼滤波干的事
    KF就是卡尔曼滤波,算法的输入值是一个可测的量,这个量可以是任何量反正得你能测量出来,而且还知道这个测量值的精度大概在多少,有了这个测量值我们就根据测量值来估计这个系统的真实输出,并同时给出我新估计的这个值的精度大概在什么范围内,这就是卡尔曼滤波做的工作,但这个工作是不断进行的,对系统不断测量,然后不断估计,这样持续一段时间之后就能估计出系统一个非常准确的输出值。这里要明确的一点是,测量值可能非常不准确,估计值也非常不准确,这符合工程中的很多工作状况,但仅仅根据这两个不准确的值最后就可以估计出一个相对准确的系统输出值,这也就是卡尔曼滤波的作用。

    下面就来具体讲卡尔曼滤波是咋运行的

    网络上有很多例子,小车啊室内温度的估计还是估计小兔兔体重啥的,但这里为了形象我还是要借用一下这个小车的例子,方便我们讲原理:

    有一辆小车车,在一个水平轴上向右行驶,它的初始位置我是用脚测量的,非常不准,我也只能确定这个位置在一个高斯分布的范围内,如图:
    在这里插入图片描述
    当然了我还有眼睛,大概能看出来这车的速度,现在我就可以根据我刚刚脚测得的位置和我目测的速度,大概估计下一秒他的位置,它大概运动到了这个位置:在这里插入图片描述
    这图非常合理,为啥呢,你会看到高斯分布的方差变大了,因为这是我估计出来的位置,脚测量和眼睛测量的误差叠加在一起所以导致我估计的这个位置的精度更加不准确,所以它是一个更加“肥胖”的高斯分布,但没关系,我再用脚测量一下不就好了很简单,好了我又用脚测量了一下,如图:

    在这里插入图片描述
    这里黄色的是我估计的位置分布,蓝色的是我此时重新测量的,那车车真实的位置在哪里呢?不知道,我永远不会知道,但是我可以同时相信这两个位置分布,然后估计出一个最优的位置分布,这里蓝色的高斯分布更加瘦,所以就更加值得我信赖,所以我会信它更多一点,那我们最终确定车车的位置就在这里:
    在这里插入图片描述
    图中绿色的部分就是我现在能信任的最准确的小车的位置,它是由我估计出来和测量出来的值共同估计出的,也就是卡尔曼滤波的输出值,而下一步我会根据这个最优估计值进行估计,(也就是我前面讲的“脚测”,但是这个时候脚的估计值精度已经明显提高了)然后再用脚测量一次,然后根据两个值的“可信赖度”去决定我信谁更加多一点,然后估计出一个最优估计值,那你就会问了,我怎么知道我该信谁多少呢?具体要信多大的量呢?这也就是卡尔曼最tm牛逼的地方,他给出了卡尔曼增益,告诉了大家每次估计完成之后应该更偏向于哪个值具体多少。

    前方劝退警告

    如果只想对卡尔曼滤波有一个感性的认识可以直接跳过这一部分去看我最后的系统建模,但是我觉得如果想对卡尔曼滤波有个深入的理解就跟我一起耐心地把整个过程推导一遍(主要是因为老子写得很辛苦,最好看一下),虽然看起来非常多,但是慢慢啃下来肯定会有收获的,这里需要有一些矩阵求导和控制原理相关的基础,如果不太懂可以去查一下,我也会尽量用通俗的语言来讲每一步:

    正式开始推导

    首先要问问自己我们到底想要啥,是不是就想要每一个时刻(k时刻)的系统真实状态,这里就用X(k)来表示k时刻系统的真实状态矩阵:
    在这里插入图片描述
    这里面A是状态转移矩阵,w(k)是噪声矩阵,认为是高斯分布的,X就是我们想要的每个时刻的系统状态的真实值,这个值永远不能得到,U是系统的输入向量,这个式子不是卡尔曼滤波的内容,这其实是马尔科夫提出的理论:一个系统的某些因素在转移中,第n次结果只受到第n-1次的结果影响,这里很好理解,如果知道前一时刻的系统状态,那我们可以根据这个系统的变化方式计算出这一时刻的系统状态。这里怎么去理解状态转移矩阵A,简单来说就是这个系统的变化方式,比如如果系统里变化量我们认为是室温,那X(k)就表示k时刻的室温,而状态转移矩阵这个时候就是1,因为我们认为温度是不变化的,如果X(k)是小车的运动速度、加速度、位移,那A就是速度和加速度的变化关系,位移和速度还有加速度的变化关系,就是那些高中的运动公式。

    下面我们在k时刻对系统的真实值进行测量,也就是下面公式里的Z(k),这时候的测量值就是我们可以得到的了,H是测量矩阵,比如我们要测的是小车的位移,那给状态变量矩阵左乘一个测量矩阵就得到了我们要的位移。v(k)是测量误差,也是高斯分布。
    在这里插入图片描述
    到这里为止,上面的两个式子都不是卡尔曼滤波的内容,上面是两个一定成立的等式,下面就要开始正式卡尔曼了:

    我们先看他的第一个等式:
    在这里插入图片描述
    这里面X(k-1|k-1)表示前一时刻系统的卡尔曼滤波最优估计值,咦,你会问,这还没开始预测呢哪来的最优估计值,因为卡尔曼滤波本身就是一个循环迭代的过程,每次估计都要用到上一次的最优估计值,(第一次用的是测量值,随便估计一个就可以了),所以就先假装已经有一个了(后面再讲怎么来的),而X(k|k-1)就是我们根据上一刻的最优估计值左乘一个状态转移矩阵再加上控制量得到的,这里因为我们已经估计值了所以没有噪声项。
    卡尔曼滤波的第二个等式:
    在这里插入图片描述
    这个实际上是一个假象的式子,先宏观来看,卡尔曼这个式子实际上就是用我们根据上一时刻估计的系统状态来修正一下,最终得到此刻系统输出的最准确的估计值,怎么估计呢,就是最开始那个小车的例子了,先假设有一个卡尔曼增益K(k),这个增益会在此刻衡量人为估计值和测量值哪个更可信,然后做一个决策,得到最终最优卡尔曼估计值,在这里我们先不要纠结这个值到底该怎么取,这一步要先理解整个式子是怎么假设出来的就可以了,简单来说就是假设了一个参数去衡量我该信哪个更多得到最终的估计值。

    在引出卡尔曼滤波的后三个关键等式之前:

    先来看看卡尔曼究竟是如何取到这个卡尔曼增益,最终让估计值更加精确的,要明白我们在干啥,看回最开始的小车的例子,我们每次希望得到的是最后这张图:
    在这里插入图片描述
    也就是说我们想要的是绿色的这个分布,我们不仅要得到位置,还要求位置的精度足够高,就相当于我们希望绿色高斯分布的方差尽量小,这里记P(k|k)是k时刻我们估计出的最优值的协方差,好了,我们的目标就是:最小化P(k|k)。

    那P(k|k)哪里来?

    所谓卡尔曼最优估计协方差就是最优估计与真实值的误差,那们记e(k|k)为k时刻最优估计的误差(后面e(k|k-1)是根据前时刻的估计值误差),显然该误差等于最优估计和系统真实输出的差即:e(k|k) = X(k)-X(k|k)

    但X(k|k)可以用卡尔曼滤波的第二个等式表示,所以对e(k|k)进行化简:在这里插入图片描述

    化到这一步以后,你会发现K(k)也就是卡尔曼增益依然存在,还是没有解决问题,没有告诉大家这个值该怎么取,但是这里出现了一个有意思的东西了:(X(k)-X(k|k-1)),这是啥玩意?刚刚我们说X(k)-X(k|k)是k时刻最优估计的误差,那这里显然就是人为估计的误差,也就是我们说的修正之前的误差,也就是说,这一步可以看出来,可以用修正前的估计误差和卡尔曼增益得到最优估计的误差。

    那下面就来计算最优估计协方差:
    在这里插入图片描述
    可以看到从第四个等号开始用了P(k|k-1)来代替E[(X(k)-X(k|k-1))(X(k)-X(k|k-1))^T],直接用修正前的估计协方差来替代这一坨乱七八糟的。那现在目标就很明确了,现在就要来最小化这个最优估计协方差,问题就转化成:将卡尔曼增益视为自变量,使得这个矩阵的迹最小时的卡尔曼增益就是我们要的K(k)。(最后一个等号把白噪声的协方差用R表示了,化简过程中要注意白噪声的期望是0,所以中间两项消去了)

    那我们就来取最优估计协方差矩阵的迹:
    在这里插入图片描述
    求最小值就顺便对K(k)求个导:
    在这里插入图片描述再顺便让导数等于零:
    在这里插入图片描述
    然后顺便把K(k)拿出来:
    在这里插入图片描述
    好了,到这里终于得到了我们朝思暮想的卡尔曼增益,可以发现是由未修正估计的协方差和测量误差的协方差决定的,以上也是卡尔曼滤波后三个等式中的一个,到这里,我们只要想个办法求出未修正估计值的协方差就大功告成了。同样的我们记e(k|k-1)为未修正估计值的误差,而且e(k|k-1) = X(k)-X(k|k-1),这步很简单就不讲了。

    同样的,求该协方差值:

    在这里插入图片描述
    第二行开始把k时刻系统真实输出用k-1时刻的真实输出来表示,同样把噪声的期望认为是0,这样就化简到了倒数第二个等号,可以看到又出现了一个神奇的东西!X(k-1)-X(k-1|k-1),这不就是e(k-1|k-1)吗,这个东西我们刚刚算过是不,那就替换过来,就有了红色的等式,噪声协方差用Q表示,这时候就很神奇了,估计协方差和前时刻的最优估计协方差是一个转移矩阵平方的关系,顺便加上噪声协方差,所以到这里为止我们可以用前时刻最优估计协方差来表示现在的估计协方差,也可以用现在的估计协方差表示现在的最优估计协方差,所以很明显,协方差的估计可以用来迭代了,红色的等式也就是卡尔曼滤波的第四个等式,就快要胜利了。

    整个完整的卡尔曼滤波的迭代过程这里已经可以写出来了,但你问为啥这里只有四个式子还少一个呢?其实我们最开始已经有第五个式子了,只是还不够好看,接下来就把刚刚没做完的做完:
    在这里插入图片描述
    这里就是把括号打开了哈,仔细化一下很简单的,然后把右边的卡尔曼增益项用刚刚的式子替代:
    在这里插入图片描述
    就化简出了一个非常简单的结果了,这就是卡尔曼最优估计值的协方差,只用估计协方差和卡尔曼增益就可以表示出来了,这里R被消去不是说跟估计值的协方差噪声没有关系了,而是把R放进卡尔曼增益里表示更加简洁,这也就是卡尔曼滤波的第五个式子了!到这里完整的卡尔曼滤波已经被推导出来了,喜大普奔!!!完结撒花!!!

    总结一下
    卡尔曼滤波的五个等式:
    在这里插入图片描述
    前两步是根据上一个最优估计值得出此刻的估计值和估计值的协方差,紧接着就可以得到此刻的最优估计值和最优估计值的协方差,然后利用此刻的最优估计值和最优估计值的协方差进行下一个迭代,完美。

    恭喜跟我看到这里的童鞋们,我真是谢谢您嘞,说明我也没白忙这么半天。

    我们可以再回头看一下卡尔曼滤波的五个等式,看起来有点多,但是实际上非常紧凑,推理非常严谨,但是有一些我也没想清楚卡尔曼究竟是怎么想到要这么化简的,觉得是巧合但又不是,只能感慨数学的伟大,卡尔曼的牛逼。

    开始系统建模
    这一部分开始最好随时把刚刚推导出来的五个卡尔曼滤波的等式放在一个方便看的地方,配合我的模型来边看边理解会比较好。

    下面就根据刚刚推导出的五个卡尔曼滤波的公式来建立一个simulink模型,模型如下:
    在这里插入图片描述

    这个模型要怎么看,就分两部分看,上面半部分实际上就是卡尔曼滤波的前两个公式,也就是预测值更新的过程,下面一大坨都是卡尔曼增益更新的迭代过程,每个节点我都把具体的变量标清楚了,仔细看是可以看出卡尔曼滤波五个公式的影子的,这里我默认是不加入控制量的,也方便后面讲整个系统的响应,所以我这里的状态转移矩阵和测量矩阵都是1,也就是用卡尔曼滤波来预测像温度变化这种简单的物理量变化,温度变化一般就认为是不变化的,所以状态转移矩阵也就是1,而温度也是直接可测的,所以测量矩阵也就是1。

    所以有了模型以后就很清楚了,卡尔曼滤波的五个等式看起来参数非常多,但大多数参数都在不断的循环更新中,所以整个系统真正能调整的参数只有三个,就是系统开始循环的初始值,这个要人为定一个。还有就是最最重要的两个参数,过程误差和测量误差的协方差,这就比较奇怪了,能调整的参数居然是两个噪声的协方差?!但是要知道过程误差和测量误差很多时候是不可得的,是系统本身就有的,所以我感觉这就是卡尔曼滤波很神奇的地方,即便是对误差的协方差有一个人为的不准确的判断,也能对系统的真实状态有个比较好的预测。

    下面我们就来对卡尔曼滤波的完整系统进行一个深入的研究,首先还是要把系统建立得更完整一点,我们给定一个真实系统,加上一个噪声,再对真实系统的输出进行测量,再加上测量噪声,将总的测量信号接入卡尔曼滤波,然后同时检测系统真实输出和测量值还有卡尔曼最优估计值,完整系统如下:
    在这里插入图片描述

    然后加入一个比较经典的阶跃输入信号,先去除所有噪声,系统过程噪声和测量噪声,包括卡尔曼滤波内部的所有协方差值都设置为0,看看卡尔曼滤波的输出:
    在这里插入图片描述
    这时候只有测量值可以检测到,没有卡尔曼输出,原因灰常简单,当R等于0的时候,卡尔曼增益等于测量矩阵的逆,这样P(k|k)=[I-K(k|k)H]P(k|k-1)就永远等于0,卡尔曼输出没有任何意义,所以要给R赋一个很小的初值才能认为传感器的精度非常高。同样的,先验误差如果等于0,也就是Q等于0,此时卡尔曼增益等于0,也就是说不考虑测量值,卡尔曼滤波只相信自己的估计值,但是你想想哈,卡尔曼滤波的输入就是测量值,你又不信它,太过分了吧,您凭空想象吗,所以这个时候的所有估计值都是没有任何意义的,即便我们希望某个误差很小,也必须是一个不等于0的值。

    那首先我们将先验误差和后验误差的协方差设同时设置为0.001,同样输入一个阶跃信号,看卡尔曼滤波的输出:
    在这里插入图片描述
    你问这跟刚那个图不是一毛一样么,不是的哈不信你翻回去瞅瞅,刚那个是蓝色的,这时候卡尔曼滤波已经有输出了,只不过卡尔曼滤波的输出和测量值重合了,为啥呢,也好理解,看卡尔曼增益:
    在这里插入图片描述
    其实这个时候先验误差影响是不大的,在传感器误差协方差接近于0的时候,就是我们认为测量误差非常非常小的时候,也就是认为传感器是一个神仙传感器,它测量出来的值是绝对真实可信的,这是一个最准的传感器,于是卡尔曼增益这里分母的R是一个高阶小量,分子约掉,最后卡尔曼增益等于测量矩阵的逆,最终看回卡尔曼滤波的第二个式子,会发现卡尔曼滤波的最优估计值永远等于测量值,这很好理解,算法都认为测量值准确无误了,那必然每次估计出的值只要相信传感器就好了呀,所以在这种情况下,它会复制传感器的值。

    下面我们固定先验误差的值不变,保持0.001,然后改变测量误差协方差的值,改变幅度很小的时候其实变化也并不大,我这里直接展示改变到10和1000的变化较大的时候。

    当R为10时:
    在这里插入图片描述
    会发现最优估计值和测量值开始有一点偏移,这里实际上也是比较好理解的,增大了R值,系统在预测过程中对测量值的信赖度降低,所以接近测量值的速度会降低,同样的把R值增大结果类似,下面R变为1000:
    在这里插入图片描述
    可不就是么,收敛速度会降低,如果固定R不动,增大先验误差的协方差道理也是一样的,会使收敛速度加快,因为增大Q的同时也就是减小了R,卡尔曼增益就更大了,卡尔曼增益变大,测量余量影响的比例就越大,所以测量值起到的作用也就越大,到这里Q和R的互相关系也就差不多描述清楚了。总结一下就是Q和R都不能为0,卡尔曼滤波的输出和先验误差、测量误差的协方差的具体值是没有关系的,和它们的比值高度相关,当然这都是从原理出发,实际在使用的时候还是要对传感器的漂移有个实际的估计。

    但是到这里为止,系统和传感器都是不真实带有误差的,我只不过在卡尔曼滤波器的内部主观加上了误差的协方差,下面就来测试一个真实带有噪声的系统。现在给系统过程误差加上0.01的方差,传感器本身也带有0.01的方差。系统输出和测量值如图所示:
    在这里插入图片描述红色线是卡尔曼滤波的值,还没加进去,蓝色是测量值,橙色是系统的真实输出,可以看到测量值围绕真实值上下飘动,下面开始用卡尔曼滤波预测,首先假设卡尔曼滤波内部对先验误差和测量误差的估计都是准确的,这里设置Q和R都是0.01,卡尔曼估计结果如下:

    在这里插入图片描述
    红色线围绕真实输出上下飘动,放大点可以看出来卡尔曼滤波可以得到一个比测量值更加接近真实值的输出:
    在这里插入图片描述

    这里我直接讲一个我的实验结论,就是刚刚我提到卡尔曼增益和单独的Q和R取值关系不大,只和它们的比值有关,所以这里如果同时给R和Q增大相同的倍数,卡尔曼滤波的估计结果都差不多,和真实输出的残差范围也基本没有变化,这个结果就不在这里展示了,直接看上面的图就可以,结果都类似于上面这个实验,这里我就展示一下输出结果有明显不同的实验。

    (下面的仿真对系统真实的过程误差和传感器的真实测量误差都没有改变,只改变卡尔曼滤波里的方差)

    下面固定Q不变,增大或者减小R值观察卡尔曼滤波的结果。

    首先固定Q为0.01不变,将R增大到1:
    在这里插入图片描述
    放大来看:
    在这里插入图片描述

    卡尔曼滤波的估计值收敛速度明显变慢了,但收敛后的值也比较平缓,原因是R增大,卡尔曼增益减小,对传感器的输入值不能足够相信,但更信任自己的估计值,所以每次卡尔曼估计出的结果都不会有太大的振动,因为每次都是根据上次自己的估计值来估计的,而且自己很信任自己的估计,所以很平稳,只是收敛速度慢。

    下面固定Q为0.01不变,将R减小为0.0001
    在这里插入图片描述
    放大看:
    在这里插入图片描述
    会发现看不到测量值了,其实这是收敛速度过快的表现,此时卡尔曼增益非常大,所以卡尔曼滤波系统内部对测量值的置信度是非常高的,几乎与测量值完全重合。

    做到这里就基本上可以概括卡尔曼滤波里两个重要参数对滤波器的影响了,最后来看一个正弦波输入卡尔曼滤波时R值逐渐增大的效果,不详细解释了,如果看到这里应该可以对照前面的平衡式自己看看哈,直接上图加深理解好了:R值分别为0.0001,0.01,1,10
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    最后再来总结一下卡尔曼滤波里先验误差和传感器误差的参数调节

    ①Q和R都不能取0,Q取0时意味着完全不相信测量值的输入,所以系统输出值衡为初值;R取0时意味着无条件相信测量值,所以卡尔曼滤波的输出与测量值完全重合,这两种情况下卡尔曼滤波毫无意义。

    ②讨论Q和R的单独取值同样意义不大,因为卡尔曼增益是受R和Q的比值影响的,在调节参数的时候要同时比较两个协方差的大小。

    ③卡尔曼增益的值越大,意味着越相信测量值的输出,收敛速度越快,最优估计值震荡越明显;卡尔曼增益的值越小,意味着测量值的可信度越低,越相信系统本身的估计值,最优估计值收敛速度越慢,输出越平稳。

    好了,终于完结,再撒花。

    有啥不太对的地方欢迎讨论哈,谢谢大噶!

    只收藏不点赞,生娃没有屁股蛋!

    展开全文
  • 卡尔曼滤波原理及仿真卡尔曼滤波原理及仿真卡尔曼滤波原理及仿真卡尔曼滤波原理及仿真卡尔曼滤波原理及仿真
  • 卡尔曼滤波原理及应用-MATLAB仿真(PDF书和配合的程序),MATLAB代码还有注释
  • 卡尔曼滤波原理及应用仿真
  • 黄小平 卡尔曼滤波原理及应用 书籍自带的matlab程序 全部章节
  • 卡尔曼滤波原理及应用——MATLAB仿真,黄小平pdf版,本书主要介绍数字信号处理中的卡尔曼(Kalman)滤波算法及在相关领域应用《卡尔曼滤波原理及应用:MATLAB仿真》编辑推荐:《卡尔曼滤波原理及应用:MATLAB仿真》是...
  • 基于自适应卡尔曼滤波原理的车距预测研究
  • 捷联惯导算法与卡尔曼滤波原理讲义,惯性导航,多传感器融合。
  • 卡尔曼滤波原理及应用MATLAB仿真 <br/>《卡尔曼滤波原理及应用:MATLAB仿真》编辑推荐:《卡尔曼滤波原理及应用:MATLAB仿真》是作者精心为广大读者朋友们编写而成的此书。《卡尔曼滤波原理及应用:MATLAB仿真》...
  • 本文档是黄小平的卡尔曼滤波原理及应用_matlab仿真的书籍pdf扫描版,内容完整,书中有相应的各种kalman滤波的matlab代码示例。书中介绍了MATLAB仿真基础、Kalman滤波原理、Kalman滤波在视频图像目标跟踪中的应用、...
  • 黄小平卡尔曼滤波原理及应用的源程序,程序内容根据书籍的目录依次排列,且有程序说明。
  • 卡尔曼滤波原理及应用 MATLAB仿真pdf源程序
  • 卡尔曼滤波原理及应用 MATLAB源程序
  • 卡尔曼滤波原理及应用——MATLAB仿真,黄小平版的matlab代码

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 4,369
精华内容 1,747
关键字:

卡尔曼滤波原理