卡尔曼滤波的五个步骤器的作用

卡尔曼滤波的五个步骤简单来说僦两个步骤:
1是预测这一步通常是根据上一时刻的估计值预测下一时刻的值,成为先验估计同时这一步会预测下一时刻的误差,成为先验误差

2是更新,也称为矫正这一步首先计算卡尔曼增益(一个权衡先验估计与测量值的权重),再利用上一步的先验估计计算后验估计同时更新先验误差到后验误差。

大概就这样具体线性滤波或者是非线性滤波需要根据你自己的场景确定,有几个比较难的点提一下:


1昰矩阵A和H的确定矩阵A预测步中上一时刻与下一时刻连接矩阵,H是测量输入值与测量输出的链接矩阵很明显这两个矩阵跟实际运行的系統有关,很难给出百搭的款
2是矩阵Q和R,分别对应预测误差和测量误差矩阵同样也很难搞这两个矩阵,不过这两个有比较多的研究像ALS估计可以帮忙确定R,那么Q的话需要根据经验确定一般根据你的需要保留的精度来。

那么滤波的输入和输出分别是什么呢


输入就是你要過滤的数据,输出就是你过滤修正过的数据了

希望可以帮到你,谢谢~

一、卡尔曼滤波的五个步骤器要解决的问题

  首先说一下卡尔曼滤波的五个步骤器要解决的是哪一类问题这类系统应该如何建模。这里说的是线性卡尔曼滤波的五个步骤器顾名思意,那就是线性动态的离散系统这类系统可以用如下两个方程来表示:

  x(n)表示系统的状态

  F(n+1,n)为状态转移矩阵,表礻状态随时间的变化规律通俗的讲,从当前状态到下一个状态之间有什么关系

  C(n)表示观测值与状态的关系

  y(n)表示状态的观测值

  v1表示系统过程的噪声

  v2表示观测过程中产生的噪声


  上面的两个方程中,第一个方程是过程方程它表示系统状态x(n)随时间的更新过程。第二个方程为测量方程表示状态x(n)与测量结果y(n)的关系。这里我们要先对这两个方程中的概念做下解释

  首先解释下状态这个概念。状态是对系统特征进行的一个抽象由预测系统未来特性时所需要的、与系统过去行为有关的最少数据组成。

  这个概念不好理解吧!那么举个例子相信不少朋友在网上看到过有人拿来讲述卡尔曼滤波的五个步骤原理。这里房间里真实的温度就是状态它可以是一个參数,也可以是多个参数那么,用温度计测出来的值就是这里的观测值y(n)。再说一个例子假如我们要对一个运动的物体进行跟踪,那麼物体的位移和速度完全可以表示这个运动物体所组成的系统的主要特征。这时的状态就可以用一个具有位移和速度两个特征的向量来表示解释到这里,相信很多朋友已经正确理解了状态这个概念它表示的是系统客观存在的真实特征

  再说一下系统状态与其观测徝之间为什么有C(n)的存在这里它表示的是观测值与状态的关系。再拿室内测度测量来举例子室内客观真实温度(未知量)做为这个系统Φ的状态,用温度计来测量这个状态测出来的温度就是我们的观测温度y(n)。这里很可能系统状态跟其观测值不是简单的加个测量噪声的关系那么这种关系就可以用C(n)来建模。

  上面的两个方程就是线性卡尔曼滤波的五个步骤器对要解决的系统的建模。这里再次对这两个方程表示的模型做出更加通俗的解释:

  过程方程:它讲述的是系统从一个状态到另一个状态应该随时间如何变化

  测量方程:它讲述的是当前状态与当前测量值之间的关系

  也就是说我们如果对一个系统感兴趣,首先要找出这个系统的一个或者几个主要特征(状態)然后对这几个特征进行观测,并得到一组观测值通过不断的观测来认识这个系统,利用仅有的观测数据找到最优的方式来求解过程方程和测量方程这就是卡尔曼滤波的五个步骤器要解决的问题。为了便于理解后面我们只讨论只有一个特征的状态所组成的观测系統。

二、投影定理与新息过程

  现在我们已经清楚了卡尔曼滤波的五个步骤器要解决什么样的问题对于这样的问题应该如何建立模型。下面我们来看下具体用什么样的方法来最优的求解过程方程和测量方程卡尔曼滤波的五个步骤器的推导过程有两种思路:

  (1)正茭投影定理(有的地方也称为射影定理,这里后面简称为投影定理)

  卡尔曼老先生最初介绍卡尔曼滤波的五个步骤器时是使用正交投影定理来推导的后来Kailath又提出来基于新息过程的推导方法。这里打算两个都介绍一下上面说到,当对系统观测时会得到一组观测值y(n),那好我们首先考虑一个估计的问题,更具体的说是如何利用仅有的观测数据来预测状态x(n)的问题由回归分析的内容我们可以知道,由观測值y求随机变量x的线性最小二乘估计(线性最小方差估计)可以表示为:

  这时随机变量x与其估计的差值跟观测值y正交(不相关),我們就称x的估计值为x在y的上投影记为:

  其几何意义表示如下:

  好,再来思考一个问题随机变量x的真实值是我们最终想求的,但並不能真正得到如果我们转变一下思路,用前n-1个观测值来预测当前n时刻的观测值会推出什么样的结果。这里我们需要再定义一个新的概念--新息它表示当前观测值与其估计值的误差。可以表示为下面的方式:

  新息几何意义表示如下:

  再来看新息产生的过程我們得到一个观测值,就能同时得到一个对其对应的误差反之也是成立的,也就是说观测值与新息存在着一一对应的关系,那如果我们想要得到最终的系统状态也新息来代替观测值来估计也是可以的,即:

  写成递推的形式为:

   看来利用投影的思想,我们是可鉯递归的表达出来状态的预测过程的也可以说,在几何上kalman滤波器可以看做状态在由观测生成的线性空间上的投影。但如果只分析到这裏估计很多朋友可能还不能真正的理解卡尔曼滤波的五个步骤器的思想。那么我们再以新息过程来分析下如何来求解过系统状态

  認真考虑下就会发现,整个过程有两个误差一个是观测过程中的预测误差:新息;一个是状态更新过程中的预测误差(一直隐含在投影过程中没有被注意到),我们来看下它们之间的关系

  由过去所有的观测值y(1),...,y(n-1)和系统的测量方程,可以得到当前观测值y(n)的最小均方估计为

  于是新息过程还可以表示为:

  这里ε(n,n-1)是状态x(n)和其一步预测值之差

  这里R(n)为新息误差相关矩阵,代表观测信息的质量优劣、K(n,n-1)为狀态误差相关矩阵代表状态时间更新的质量优劣、Q2(n)表示测量噪声向量的相关矩阵。再由观测方向可得:

  这就是状态预测误差与新息の间的二阶统计关系因此,只要我们尽可能的优化时间更新的质量同时提高观测信息的质量,那么我们一定能估计出想要的系统状態。好问题反过来,再回到文初提出来的问题我们有了一组观测值,又得到了与比对应的新息序列如何好好利用它们估计现我们想偠的系统状态。下面就要好好说下:

  根据观测值与新息之间的一一对应关系如果我们以新息过程来估计状的最小均方估计,则这个估计可以表示为新息过程序列的线性组合即:

  这里B为待定系数矩阵,那么根据线性滤波的正交原理,预测状态误差向量与新息过程正交即:

  再将新息过程导出来状态x(i)的最小均方估计代入,可得

  这里的R(m)为新息过程的相关矩阵上式两边同时乘以R(m)的逆矩阵,鈳得待定系数矩阵B的表达式为

  再将B代入新息过程导出来状态x(i)的最小均方估计公式中可以得到状态x(i)的最小均方误差估计为:

  对于i=n+1,有

   我们看到这个结果与投影定理推导的结果的含义是相同的,只是使用的工具和准则不同

  最后,我们再回想下观测数据与噺息、状态之间的关系以及根据这些关系导出的结果。不难得出结论卡尔曼滤波的五个步骤器的出发点是尽可能的好好利用观测值,鼡尽可能少的计算量来估计系统状态呵呵,是不是感觉数字的游戏还是很好玩的并不一定很枯燥。好的今天就分析到这里吧,后续將会使用这次分析的结果逐渐对卡尔曼滤波的五个步骤器及其使用做一个说明。这里的分析内容来源于我在学习卡尔曼滤波的五个步骤器过程中的一些思考(参阅书籍:自适应滤波器原理、卡尔曼滤波的五个步骤原理与应用)水平有限,如有错误之处还请各位大牛不吝赐敎。

本文购买自 修改了公式乱码,僅供学习请支持原版购买

首先声明一下,此文是预备给初学者阅读的旨在帮助初学者更好地理解卡尔曼滤波的五个步骤器算法,降低公式推导的难度所以我尽量避免一些太过于学术化的讲解。

我在学习卡尔曼滤波的五个步骤器算法时也花了不少时间。

有时候觉得自巳弄懂了遇到新的问题时,还是不知道灵活运用然后就发现自己还是没有弄懂,自己只是因为照着别人的博客顺着别人的思路顺利阅讀完毕而已脱离了博客中的示例场景,切换到新的场景仍然不能很好地运用,反反复复之后某天闭眼也能手推卡尔曼滤波的五个步驟器涉及到的 5 个公式时,仿佛有大彻大悟的感觉然后,我才认为自己再遇到新的问题时我不会再像之前那样不知从何入手了。

先看看為什么叫“”跟其他著名的理论(例如傅立叶变换,泰勒级数等等)一样卡尔曼也是一个人的名字,而跟他们不同的是他是个现代囚!

匈牙利数学家,1930 年出生于匈牙利首都布达佩斯1953,1954 年于麻省理工学院分别获得电机工程学士及硕士学位1957 年于哥伦比亚大学获得博士學位。我们现在要学习的卡尔曼正是源于他的博士论文和 1960 年发表的论文《A New Approach to Linear

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

1. 什么是卡尔曼滤波的五个步骤器

卡尔曼滤波的五个步骤器是一种估计算法,在上个世纪六十年代就大显神威了NASA 将它应用茬阿波罗飞船的轨迹预测上面,所以它称得上是一个能上天的算法

卡尔曼滤波的五个步骤器能够对未来数据做预测,到今天它在许多荇业都发挥了重大的作用,如导航、自动驾驶、电池电量预测、雷达目标追踪、机器视觉等等

你可以将卡尔曼滤波的五个步骤器算法应鼡在你想要预测的领域,例如市场上猪肉的价格、房间里面的温度、太阳底下你影子的长度变化、你个人网站或者是博客或者是 QQ 空间的访問量等这个是灵活的,只要你愿意尝试反正它是一个优秀的估计算法,耗费的硬件资源也少灵活方便,每一个程序员都应该了解一丅

2. 如何通俗解释卡尔曼滤波的五个步骤器?

我们已经知道卡尔曼滤波的五个步骤器算法可以做估算。这个小节我进一步举例阐述,讓大家对它的逻辑有一个进一步的感性认知提神

首先,我们要明确地记得卡尔曼是用来估算的这是我们使用的目的,很多人被复杂的公式推导搞不清头脑最终忘记了卡尔曼滤波的五个步骤器算法的本来目的,从而让自己特别纠结踌躇

其次,如果我们能够有办法确定烸时每刻的事件那么就不需要运用卡尔曼滤波的五个步骤器。卡尔曼滤波的五个步骤器算法适用于不能保证能够准确确认的事件

简单來讲,卡尔曼滤波的五个步骤就是加权给不同的数据分配不同的权重,然后产生新的数据也是数据融合的意思。

比如公司叫你和小李同时评价新员工的表现。

你说这人太不靠谱了满分为 10 分的情况下最多打 6 分,小李却说人不错可以给 8 分。

那上司怎么办呢一般情况丅最简单的方法是求平均值,也就是 6+8 等于 14平均分是 7。

但一般靠谱的领导会有类似的逻辑处理事情

在领导眼中,你和小李说话的可信度昰不一样的所以他可能信你少一些,也可能信你多一点所以,最终的结果就不会是加权平均可能是 6*0.3+8*0.7=7.4。

最优数据 = 你的数据 + K(小李的数据 ? 你的数据)
卡尔曼滤波的五个步骤器也正式类似的逻辑不过做了些许巧妙的处理,首先它会计算你们的误差在这里是 8 分减去 6 分,为 2 分接着用一个比例 K,取值 0~1最终结果是两者都有考虑,但是权重不一致

如果 K 为 0,则上司完全信任你你说多少分就是多少分,如果 K 为 1 的話那么就是 8 分,代表上司完全信任小李的评价其它的情况,K 在 0 和 1 之间最终的结果是你们的分数综合,但是权重不一样

K 等于 0.5 时,两鍺权重一样

K 小于 0.5 时,偏向你反之偏向小李。

在卡尔曼滤波的五个步骤器中 K 被称为卡尔曼增益

到这里,我们基本上知道了卡尔曼滤波嘚五个步骤器大概是怎么一回事了下面我们还需要理解 3 个概念才算更进一步的理解。

王老板是做肉类批发生意的他手下有员工小 A 和小 B。

小 A 负责每天早上给王老板预测当天肉类的价格

小 B 每周一上午 9 点到专门的行政机构去摘抄数据,然后汇报给王老板

所以,王老板有 2 个數据源王老板懂得卡尔曼滤波的五个步骤器算法,所以他能够搞定价格估算这件事情。

小 A 和小 B 的数据来源不一样误差也不一样。

小 A 嘚数据是靠经验拍脑袋决定的被称为预测(predict)

小 B 的数据来源于机构的统计准确度比较高,被称为测量(measure)也被称为校正(correct)

有哃学可能会有疑问小 B 的数据很准确,那么我们为什么不只用小 B 的数据呢

关键是,条件不允许啊现实生活中,很多的东西都是被制约嘚比如,小 B 只能在周一上午能够得到数据但是其它 6 天,王老板也得做生意啊因此,王老板必须依赖于小 A 的经验预测

但是为了防止這种拍脑袋的数据结果偏差太大,所以王老板才会在适当的时候引入小 B 的数据来校正小 A 的预测,最终王老板综合两者得到了自己的估算数据,并且将它作为每天的结果

这个例子,就是典型的卡尔曼滤波的五个步骤器算法所要涉及的基本概念示例

到这一步,我们已经佷清楚知道了这个算法的概念包括预测、测量、估算,也知道需要用一个卡尔曼增益来加权调和 2 个数据形成最终的估算数据

如果是文科生,或者其他理工人员了解了这么多后,已经可以和人家谈笑风生讲这个神奇的算法了

但是,作为一个相关的软件工程师这个还鈈够的。

你还需要知道如何套用公式去求解:

  1. 预测的数据值 ,公式中用 表示
  2. 测量的数据值,公式中用表示
  3. 卡尔曼增益公式中用 K 表示
  4. 最终估算的数据值,公式中用 表示

如果你是一个算法工程师,光知道用还是不够的你得清楚知道公式怎么来的,透彻理解它的公式后遇箌新的应用场景,你才能从容不迫

不然,总是反反复复陷入一种似懂非懂的困境

3. 卡尔曼滤波的五个步骤器相关的公式

卡尔曼滤波的五個步骤器算法是时域上的线性变换估算。

在一个系统中卡尔曼滤波的五个步骤器用状态(state)来描述某个时刻的系统,它是一个向量

比如,我要用一个向量来描述我每天的健康状态我选用了最低心率、最低血压、疲劳度 3 个指标。

我可以每周去医院测量一次可以准确获得。

其它时间是我自己的预测,虽然和真实值有出入但大致还过的过去。

最关键的是我今天的预测和昨天相关,明天的和今天的相关因为状态是向量,所以很容易用线性代数的方式描述

  • t 表示当前时刻,t-1 表示上一时刻ω 表示此次预测的噪声,假定 ω 符合高斯分布

  • F 昰状态转移矩阵,因为在线性代数中矩阵表示变换,所以一个向量与一个矩阵相乘一般会得到一个新的向量。

  • F 的值与具体的问题相关实际解决问题时,要分析状态到状态变换时的关系

比如,我认为我每天的状态相差不大所以,我的 F 可以是单位矩阵

实际上,完整嘚状态变换公式应该是:

  • B 是控制矩阵u 是控制变量,它们的乘积代表外力干预

    • 比如,如果让我加班我的疲劳度会加 20,如果多放一天假我的疲劳度会减少 15。

    • B 和 U 的取值同样需要具体问题具体分析

用 表示 的协方差,有下面的公式

这是预测相关的两个公式,下面讲测量

  • H 昰状态变换矩阵,有时候测量的数据和预测的数据单位不统一这个时候就需要转换。

    • 举个例子假设让你预测今天的苹果价格,你说 10 元/斤然后,网络上的报价是 3.2$/kg这就是单位不一致,需要用 H 这样的矩阵变换过来才能有效处理。

测量的数据也需要记录协方差信息用 表礻,有下面的公式:

Rt 是测量噪声 的协方差

有了预测和测量的公式,我们还需要估算的公式在这之前我们知道,我们要求 K,也就是卡尔曼增益

最终,我们用 表示经卡尔曼滤波的五个步骤计算后的估计状态

它也有协方差,用 表示:

与卡尔曼滤波的五个步骤器相关的公式就铨部罗列出来了一字排开如下:

有了上面的公式,只要我们好好调试预测误差和测量误差我们就能套用卡尔曼滤波的五个步骤器算法來取得比较好的效果,对于一般工程师而言这已经足够了。

但是如果你是算法岗位的话,这个还不够的你需要确定下面一些问题:

  1. 公式 2 怎么由公式 1 推导出来?
  2. 公式 4 怎么由公式 3 推导出来
  3. 公式 5 怎么推导出来的?

本节的目的是便于理解为主所以整个推导可能不会显得那麼正式与严肃。

因为涉及到预测值、测量值、估计值为了避免混淆,用不同的符号表示

我们预测的状态用 表示,其实也可以理解为先驗状态

用表示理论上的绝对真值。

用 表示经卡尔曼滤波的五个步骤求出来的估算值也称为最优值,也可以理解为后验状态

无论是预測值,还是估算值与真实值之间还是存在误差的。

如上图虚线表示分别用 和 表示。

接下来我们用一个协方差矩阵 来表示预测值偏离嫃实值的状态。

根据协方差的公式可得当前 t 时刻的状态协方差为:

因为卡尔曼滤波的五个步骤器是依照时间递推的,所以 t-1 时刻的状态协方差为

我们前面已经知道,t 时刻的状态可以由 t-1 时刻的状态转移过来,也就是如公式 1 所示

那么按照刚刚协方差的公式:


因为 x 和 ω 没有什么楿关性,根据概率论知识两个独立的变量,协方差为 0所以上面的公式可以化简为:

仔细观察公式,可以看出来公式实际上就是:

这實际上就是公式 2,到这里公式 2 已经就可以证明了。

神奇的是的协方差没有参与到迭代当中。

实际上概率论中有这么一个公式。


那么通过这个公式也能直接又公式 1 推导出公式 2

我们依照同样的方式可以推出公式 4,我们有 表示测量值的协方差很容易得到:

公式 4 也证明了, 是测量误差 的协方差

最难的是公式 5 的证明

文章前面的小节我们已经有了卡尔曼增益的概率也大概知道了怎么基于 2 个数据求最优的估计。

现在预测值是 ,测量值是 我们用 来表示经卡尔曼滤波的五个步骤估算的值。

K 代表一种比例因为有 K 的存在,所以 会在 和 之间取徝摆动:

根据前面测量的公式可得:

接下来,就有一个巧妙的变换:

巧妙的是到这一步估计值的误差和预测值的误差还有测量的噪声囿了联系。

我们用 表示估计值的协方差根据协方差定义可得。

现在只剩下 K 的值没有确定了。

协方差代表偏离真实值的程度卡尔曼滤波的五个步骤器的算法就是以不断减少估计值对应的协方差,从而让估计值渐渐偏向于真实值

所以,我们以 建立一个目标函数目的就昰求它最小值时,K 的取值

我们可以求 K 相对于 的偏导数,当结果为 0 时,表示 K 是最佳的取值

K 求出来后,把它的值套入前面求出来的公式当中和 也就出来了。

5. 卡尔曼滤波的五个步骤器算法预测鼠标指针坐标

这个是比较经典的一个卡尔曼滤波的五个步骤的示例创建一个黑色背景的窗口,鼠标移动时实时显示它的位置这样就形成了轨迹。

另外还将经过卡尔曼滤波的五个步骤器算法估计的位置也显示出来

在当湔目录创建一个 build 目录。然后编译执行

绿色的轨迹是测量得到的。

为了示例的简单化我将测量矩阵和过程矩阵都设置为了单位矩阵,过程噪声也设置得比较少测量噪声就干脆没有设置。大家可以自行调整参数

大家注意看动画中鼠标急剧转弯时,绿色的轨迹马上能够反映出来而红色的轨迹则比较圆润,这也体现了卡尔曼滤波的五个步骤的平滑效果

6. 卡尔曼滤波的五个步骤器算法在自动驾驶当中的应用礻例

前面说过,卡尔曼滤波的五个步骤器的算法在很多行业都有广泛的应用只要你熟练掌握了它,那么它就能够为你所用

现在,很多高配的车上都配备了 ADAS 功能能够有效地检测道路线和其他车辆,从而潜在危险发生时车辆会预警。

那么检测车辆就成为一项必须完成嘚硬指标。

能够识别车辆的算法有很多有传统的机器视觉算法,也有神经网络

目前,威力最大的还是 SSD 和 YOLO v3尤其是 YOLO v3,检测速度非常快

泹是,有一个问题是在汽车上软件都要运行在嵌入式平台,硬件资源有限所以车辆检测耗时要比 PC 上要久。

这涉及到一个实时性的问题安装在车辆上的摄像头,每秒要处理 24 帧才算实时也就是要在 50 毫秒内完成所有的图像处理代码,很可惜的是这很难办到

所以,需要卡爾曼滤波的五个步骤器这样的算法来估计

假设算法检测到一辆车需要 200ms,卡尔曼滤波的五个步骤器预估算法只要 3ms那么我们可以每隔 4 帧用算法检测一次车辆位置,而其它时间我们用卡尔曼滤波的五个步骤器算法估计车辆位置

算法检测出来的车辆可以当做是测量数据,其余嘚就是卡尔曼滤波的五个步骤器的预估最终平均下来每一帧的处理时间不超过 50ms,也就达到了实时性的要求

这个是 YOLO 算法检测出来的,不┅定每一帧都可以检测的到我们可以看到 bbox 在闪烁,抖动比较大

这个是 YOLO 结合卡尔曼滤波的五个步骤器做出来的效果,可以看到白色的框抖动比较小起到了平滑作用。

总体上来讲卡尔曼滤波的五个步骤器要理解起来并不难。

但是要灵活运用去解决新的问题而不仅仅停留在别人的示例 Demo 中的话,需要自己下一番功夫要好好理解它。

现在将容易混淆的地方再罗列一下。

  1. 预测值(predict)这是先验概率,是靠系统的转移矩阵得到的值误差比较大。
  2. 测量值(measure)误差较小,但也存在噪声
  3. 估计值(estimate),经测量值修正的的一个值也就是后验概率。

观察各自的协方差可以发现每次预测的时候,过去的误差会累计到当前按照这个趋势,预测值会离真实值越来越远

而测量值的介入,经过卡尔曼增益的比例调和新的估计值会往真实值回归,协方差公式也说明了它是递减的。

上图中圆圈代表测量阶段的测量徝,它的介入会产生一个估计值也就是蓝色的向量,这让原本的轨迹向理论上的真实值靠拢经过不停的迭代,整个系统预测的轨迹也會越来越准这就是卡尔曼滤波的五个步骤器的美妙之处。

卡尔曼公式求证的手段和角度有很多但本质上它和最小二乘法相差不大,不哃之处是它是迭代递归的

如果,有同学对向量的协方差不熟悉可以推荐《程序员的数学 2 概率统计》这本书,如果对于矩阵求导不熟悉网络上有大量的资料可以查阅。

最后本文介绍的是最基本的卡尔曼滤波的五个步骤,它适合线性的系统但现实生活中,很多系统是非线性的解决这类问题可以用 EKF 和 UKF 两种,有兴趣的同学可以自行深入了解


我要回帖

更多关于 卡尔曼滤波的五个步骤 的文章

 

随机推荐