专栏首页行走的机械人【opencv实践】easy卡尔曼滤波中:结合一个小例子的数学推导

【opencv实践】easy卡尔曼滤波中:结合一个小例子的数学推导

我们上一篇文章:

【opencv实践】easy卡尔曼滤波上:浅谈

介绍了卡尔曼滤波的由来和原理,我们在这儿一句话总结一下,大家如果有不懂的可以去看上篇文章:

卡尔曼滤波就是将模型预测的数据和观测数据进行融合,来过滤掉观测误差,进而得到更精确的数据。

以一个小车在平面上走为例子,小车的程序让它以初速度为V0,加速度为a的运动方式向前运动,但由于各种误差和原因(比如有风,或者上坡等),我们的小车并不严格按照这个模型走,存在噪声(并且假设高斯分布)。

并且我们不断的用传感器来测量小车的位置,但传感器也存在高斯分布的噪声。

卡尔曼滤波主要有五大公式,在这个例子的基础上,我们依次来看一下。

第一个公式:

  • Xk:当前状态描述矩阵
  • Fk:状态转移矩阵
  • Bk:控制矩阵
  • Uk:控制向量

对于小车来说,状态描述就是它的一维的位置x,以及运动速度v:

状态转移矩阵就是从上一时刻到下一时刻之间的过渡:

我们将Fk和Xk-1相乘,就可以得到:

显而易见,就是在小车匀速运动下的状态变化了。

但我们的小车是匀加速运动的,我们将加速度视为控制向量Uk,因为加速度是由踩油门啊之类的产生的(姑且这么理解吧)。

因为我们只有一个控制变量,及加速度a,所以

控制矩阵Bk就是将控制向量转换,将控制变量体现到状态Xk上:

到此为止,我们用第一个公式推导出了小车从上一个时刻到当前时刻状态的理想变化。

第二个公式:

  • Pk:描述状态量之间的互相关性,是Xk的协方差矩阵,也就是Cov(Xk)
  • Fk:状态转移矩阵
  • Qk:外界噪声的协方差

Pk是Xk的协方差,我们第一个公式更新了状态向量Xk,第二个公式就是更新Xk的协方差Pk,并加上一个外界噪声Qk。

因为我们用的是卡尔曼滤波,我们目标对象的状态都是呈现高斯分布,所以我们有两个需要关注的变量:均值以及协方差。所以我们需要在更新了Xk,也就包含均值和方差之后,还需要更新协方差,来关注各状态量之间的关联程度。

第三个公式:

  • K:卡尔曼增益矩阵
  • Hk:传感器数据
  • Pk :传感器测量的噪声的协方差

第三个公式的意义就是求出K,而K是为了在公式四五中使用,单独拎出来并没有什么直观上的意义。

我们知道,卡尔曼滤波就是将传感器的观测数据和模型的预测数据相融合,而公式一二做的就是给出模型的预测数据,公式三四五做的就是将预测数据和观测数据相融合来得到一个新的,更准确的数据。

在这两者之间有观测数据的采集这一步,但这不属于卡尔曼滤波算法要做的。

那怎么融合呢?我们的预测数据和观测数据都是高斯分布,两者大概呈下图关系:

而显然两者的重叠部分就是最优的最可能的分布了,如何得到中间这一块呢?就是将两个高斯分布相乘。

两者相乘之后(也就是融合之后),得到的大概如下图:

为了简便,我们用一个k来简化一下:

这个k,就是我们公式三求的卡尔曼增益了!(一维情况下)

而后面的两个公式,就是我们公式四五的一维表达了,不过因为我们实际问题有可能是多维的,所以我们实际的公式用矩阵表示,看着可能更复杂一些。

第四个公式:

  • Xk' :数据融合之后的状态分布
  • Xk:模型预测的Xk状态分布
  • Zk:Xk的均值
  • HkXk:传感器观测数据的均值

第五个公式:

更新状态描述量的互相关性。

到此,我们的五个公式就完成了一次循环,在应用中我们就是迭代进行一步步滤波和预测的。

说是公式推导,其实就是强行给大家介绍了一波这五个公式,因为推导的话,我写不出自己的东西,因为我也是看别人的教程学的,真要写大概率就是纯抄了,意义不大。

大家觉得没看懂的想看数学推导的,可以看这篇《图说卡尔曼滤波》,外国一位大牛写的不过已经被翻译成中文了,强烈推荐。

https://zhuanlan.zhihu.com/p/39912633?utm_source=wechat_session&utm_medium=social&utm_oi=1024373037663211520

本文分享自微信公众号 - Opencv视觉实践(gh_31e12b1be0e0),作者:周旋

原文出处及转载信息见文内详细说明,如有侵权,请联系 yunjia_community@tencent.com 删除。

原始发表时间:2020-04-13

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • Hello GitHub

    想来GitHub对于理工科,尤其计算机专业的大佬们应该很熟悉了,但作为机械专业小铁渣渣,自从创建了我的GitHub账号,就从来没在上面刨过什么好东西,更没有啥拿...

    周旋
  • 二寸照片识别/切边/矫正

    大体思路可以描述为Canny边缘检测-形态学闭操作-轮廓检测-Hough直线检测-确定四个角点-透视变换。

    周旋
  • 【材料力学】二:轴向拉伸与压缩

    上图中a,b,c三个图两边受力都不同,但在杆中间截面上的内力时相同的,所以上图三种不同的组合,因为静力等效,所以可以都换为a图的形式进行计算。

    周旋
  • Hadoop学习笔记—16.Pig框架学习

      Pig是一个基于Hadoop的大规模数据分析平台,它提供的SQL-LIKE语言叫Pig Latin,该语言的编译器会把类SQL的数据分析请求转换为一系列经过...

    Edison Zhou
  • 遍历HashMap的方法(四)

    用户1112962
  • 司晓:微信政务,拥抱工具革命

    ?   以下为腾讯研究院秘书长司晓博士在世界互联网大会·微信城市服务分论坛上的演讲:   今天非常荣幸站在这里给大家做《2015微信政务民生白皮书》的发布。这...

    腾讯研究院
  • 小白博客 sqlmap之POST登陆框注入方式二【自动搜索表单的方式】

    sqlmap.py -u "http://192.168.160.1/sqltest/post.php" --forms 它会有几次消息提示: ? ...

    奶糖味的代言
  • 最新疫情数据报告已到达你的邮箱!

    2020年“地狱”难度的开局,属实一下子还没反应过来事情就发生了。截止到1月31日,新型冠状病毒肺炎累计确诊的人数已是“非典”的两倍之多,广大被“禁足”的网友们...

    Rocky0429
  • 生信流程大全-基于nextflow的nf-core

    题目:The nf-core framework for community-curated bioinformatics pipelines

    生信技能树
  • 打包利器webpack

    Webpack 是一个模块打包器。它将根据模块的依赖关系进行静态分析,然后将这些模块按照指定的规则生成对应的静态资源。

    笔阁

扫码关注云+社区

领取腾讯云代金券