Kalman Filter 学习笔记
Intro
最近在学习伟大的Kalman Filter,这篇笔记主要是对国外的两篇介绍Kalman Filter的博客的翻译和一些个人理解,博客链接:Kalman Filter For Dummies, How a Kalman filter works。两篇博客的侧重的层次面不太一样,第一篇讲的层次比较浅显但是缺失了一些细节,第二篇讲的很深入但是有些抽象,这篇笔记将两篇博客结合起来,希望可以由浅入深的理解Kalman Filter。
Kalman Filter的简介和应用
首先先介绍一下Kalman Filter(以后简称kf)。kf是一种线性二次估计算法,由Rudolf Kalman于1960年提出,在随后的阿波罗计划中的导航系统就主要应用了kf算法。kf用对系统在时域的测量来预测一些未知的变量在下一个时间点的状态,测量的对象包括噪音干扰和其他的不确定因素。kf相比于其它的预测算法有更高的精准度。
kf可以用于任何包含未知信息的动态系统,然后自适地预测系统下一步的状态。哪怕环境的干扰很多很复杂,kf一般也可以得出很准确的结果。kf在连续变化的系统有很好的效果。它的优点是在预测时并不需要记录很多系统过去的状态,只需要知道上一状态的预测结果的误差协方差矩阵就可以了,所以kf可以算得很快,可以用于实时的动态系统。
一个kf的小例子
首先我们创造一个场景来引入kf算法。假设我们有一个机器人可以在丛林中穿梭,机器人的控制算法需要知道机器人的位置和速度然后才可以导航机器人的运动。我们用一个状态Xk向量来表示:
这个例子为了简化所以只考虑两个状态变量,在现实中可能会有更多的变量,更多的变量就意味着更多的信息,这更有利于kf的计算精准度。
同时,这个机器人还有一个GPS传感器来测量距离和速度。这个GPS的精度在10米左右,但这是远远不够的,因为如果在丛林里有一个悬崖的话10米的精度并不能保证机器人可以及时的避免。所以单单依靠GPS的数据来控制是不够精准的。而且传感器的也会被干扰,所以传感器的数据并不是完全可依靠的。
机器人的运动可以分为主动因素和外界干扰两个方面。主动因素是控制程序给电机发送的运动指令,它可以是向前向后,但这并不能保证机器人一定会沿着指定的方向运动,因为还有外界干扰的存在。比如说机器人遇到障碍了,被风吹了等等。所以外界干扰也是不可被忽视的一部分。
在这里就可以引入kf算法中的两个概念,主观预测和客观测量。我们的主观预测就是基于我们对机器人发出的命令,如果是向前5米,那我们对下一个时间点的的主观预测就是向前5米。这个预测当然是不准的,因为我们没有考虑到外界的干扰。客观测量就是我们通过GPS得到的数据,首先这个数据是间接的,因为传感器的得到的数据只是一些电平信号,我们还需要将它们对应转换到现实世界的数据。其次这个数据也是不可靠的,理由见上。
我们对机器人下一个状态的最优预测只能基于主观预测和客观测量这两个信息,但是哪一个更可靠一点呢?我们加入权重来让这个预测结果更公平一点,这里就引入了我们最优预测的权重公式:
其中Zk是我们得到的客观测量值,X(k-1)就是我们上一次做出的主观的预测,Xk就是我们最终的预测值,而K就是我们要通过kf找到的权重值,又称Kalman Gain。这样得到的估计相比于独立的主观估计和客观估计都要精确的多。
但是注意,这个最优预测并不是用于实际的使用,实际中还是使用主观预测为估计,但是当到下一个时间点得到客观测量后,我们可以修正我们的估计,得到最优估计,这个估计是作为参数传递给下一次估计,这样一次次修正,我们的估计会更加精确。
分布函数与协方差矩阵
在我们的问题情景里,我们只有两个需要追踪的状态,位置和速度。我们可以假设位置和速度都服从于高斯分布,kf算法中,都假设要追踪的变量服从于高斯分布。一维高斯分布的分布函数为:
中间的μ是这个高斯分布的期望,σ是标准差,σ^2是方差,在这个分布里只有一个变量,这个函数的积分为1。现在我们把高斯分布扩展到二维,则分布函数为:
在这个图像中,X轴Y轴分别代表一个变量,Z轴是概率,所以中间最高点对应的X,Y为这两个变量的期望,整个凸起部分的体积积分为1。用2维的图来表示则为:
颜色越白的地方概率越大,所以期望在中心点,周围黑色的部分概率为0。注意上图中所标注的方差的大小不是所标注的范围,此处有误解。
在以上的分布的例子里,位置和速度的相关性不是很强,但是在现实中,位置一般是和速度有很大相关性的,比如以下的例子:
在这个例子中,速度与位置成正相关,当速度大的时候,往往位置也会大,速度小的时候,位置也会小。这样的相关性信息也是kf算法所需要的。多维变量的相关性的信息是用协方差矩阵进行储存的。协方差矩阵可以储存一个一维列向量的变量之间相互的相关性信息。在这个例子中一维的变量向量和协方差矩阵为:
自己和自己的相关性是1,所以Σpp和Σvv都是1。相关性与顺序无关,所以Σpv等于Σvp。所以协方差矩阵Pk是一个对称矩阵。
主观预测与状态空间方程的构建
首先要构建的是状态空间方程,所以变量为位置和速度。k是时间变量,我们的主观预测需要从(k-1)时的状态来预测k时的状态:
蓝色的区域是(k-1)时的速度与位置服从的分布,紫色的是k时速度与位置服从的分布。这个图假设不同的时间点速度与位置服从的分布不同,然而在大部分时候,速度与位置的分布是不随时间变化而变化的。
在这里,我们做的是主观预测,我们的预测是给予一个经验公式,这个公式可以来自于控制的命令,或者其它的一些我们已知的信息。我们的预测需要将(k-1)的状态的分布转移到k的状态的分布,每一个(k-1)的点都可对应到K时的一个点。假设我们状态转移矩阵是Fk则:
注意,一般情况下,这个Fk就是1,因为分布的关系不会随时间变化而变化。
我们可以把这个state space 方程写下来:
Δt是k-1到k的时间变化,在这里假设机器人匀速运动。这样我们就得到了我们的初步的主观预测方程。我们可以从(k-1)的状态预测下一个时间点时位置和速度的值。同时根据协方差矩阵的性质:
我们可以得到一个完整的预测信息:
但是这个状态方程只是一个很基础的,我们还需要考虑其他因素对系统的影响,比如说电机对机器人的控制,外界干扰等等。
我们首先考虑电机对机器人的控制,如果加上电机的加速度,则空间状态方程为:
Bk就是控制矩阵,uk就是加速度向量。
此外还要考虑噪声的影响,如果加上噪声,则每一个(k-1)时的点不会对应到K时的一个点,而是对应到一片区域,所以对应关系如下:
因为每一个点都对应到一片区域,所有的对应叠加在一起,得到的新的K的分布范围应该比以前要大,如下图所示:
面积明显比上图要大,同时我们假设噪声是高斯白噪声,所以期望为0,所以加上噪声后K的期望不变。假设噪声所造成的那个小绿圈的速度与位置的协方差矩阵为Qk,则新的主观预测和协方差矩阵为:
通过这个方程我们可以在X(k-1)时主观的预测出Xk时的状态。下一步就是得到客观测量的结果。
客观测量
我们的机器人上有GPS传感器可以得到位置和速度的信息,假设sensor1测的是速度,sensor2测的是位置:
但是注意,因为传感器得到的只是电平值,所以得到的分布函数的范围与现实中的速度和位置的范围不一样。我们需要用一个转移矩阵把现实世界的速度位置数据的map到传感器的电平数据单位,每一个点对应传感器坐标的一个点,然后在相同的单位下进一步计算权重Kalman gain。这里我们引入转移矩阵Hk:
对应的期望与协方差被转换为:
这里的μ expected就是我们的主观估计在传感器的单位下的值,Σ expected则是我们的主观估计的协方差矩阵在传感器单位的转换后的矩阵。
当然,我们也不能忽略传感器的噪声影响,噪声会造成我们在把现实世界的值转换为传感器单位时一个点对应传感器坐标系的一片区域:
所以当传感器坐标系中每一个点都被这样的一个绿色的小椭圆代替,新的传感器坐标系下的数据分布将变为:
形状变成这样是因为我们假设的传感器噪音是这样的一个椭圆形,原来的紫色的小圆形被扩张成了这样的绿色椭圆。
则绿色椭圆分布的期望(中心点)就是我们传感器读到的客观测量数据,但是包含误差。
得出最优修正预测
得到我们的主观预测在传感器坐标系的分布和客观测量得到的数据的分布后,就可以计算权重了。首先我们先将两个分布重叠在一起,得到如下分布图:
紫色的是主观估计,绿色的是客观测量。所以我们就有了两个概率:
1,我们的主观估计是我们最优估计的概率
2,客观测量是我们最优估计的概率
在这里我们直接将两个概率分布函数相乘,得到一个新的分布。因为黑的地方概率为0,所以相乘后紫色和绿色的非重叠面积都变为0了。我们只得到中心重叠的区域为我们最优的预测的分布:
这个分布的期望就是我们对下一个时间点K的最优预测。这个预测的特点是,在这个点上,主观估计和客观测量的概率最大。
一个一维更直观的图示为:
那个小蓝色分布的期望就是红色分布和绿色分布同时预测时的最佳预测值。
计算权重Kalman Gain
我们首先考虑只有一个变量时的情况。这时分布的波动情况用方差表示。
因为我们是直接将两个分布函数相乘得到一个新的分布,所以用公式表达为:
如果两个分布都是服从高斯分布:
则相乘以后得到的新的分布的期望和方差为:
我们可以在期望的计算公式中提出一个K,作为我们的权重Kalman Gain:
下面我们考虑多个变量的情况,这时数据的波动情况就不能用方差表示了,应该用协方差矩阵来表示,所以直接替换得到的新的分布和新的K:
这样,我们就得到多变量时的Kalman Gain。这个分布期望就是我们最终的预测。协方差矩阵就是我们要传给下一次预测的信息。
推导公式
我们的主观预测的分布是:
客观测量的分布是:
所以代入刚才得到的最佳预测的分布公式得到:
K‘就是我们的最佳预测的权重Kalman Gain,Xk’就是我们的最佳预测,Pk‘就是我们要传给下一次预测的协方差矩阵。至此一次预测已经完成。
迭代流程图
我们首先给以个起始估计,然后在(k-1)时先做主观估计,用主观估计值作为我们的估计,然后等到K时,用得到客观测量进行矫正我们的估计,再把矫正后的新的值作为这次的最佳估计和新的协方差矩阵传递给下一次估计。
Time Update是在(k-1)阶段,Measurement Update是在K阶段。等到K时我们才可以得到客观测量值,所以是用客观测量进行矫正之后的最佳估计值再用来下一次估计。但这个最佳估计并没有被用在实际中,之前实际用到的估计还是Time Update的估计值。
简单的计算例子
参见A Simple Example
作者:凌晨2点的北京 链接:https://www.jianshu.com/p/2768642e3abf 來源:简书 著作权归作者所有。商业转载请联系作者获得授权,非商业转载请注明出处。
Kalman Filter 学习笔记相关推荐
- Dr_can Kalman Filter学习笔记(三)
Dr_can Kalman Filter学习笔记(三) 本文学习自Dr_can卡尔曼滤波关于Kalman Gain的推导 本文对卡尔曼增益进行一个详细的推导. 一.问题引入 在笔记二中我们得到了状态空 ...
- Quaternion kinematics for the error-state Kalman filter 学习笔记
1.卡尔曼滤波器的核心是追踪状态变量的均值和方差在一个动力系统里如何变化,以及计算关于测量值的条件期望和条件方差.在 IMU 的姿态估计中,姿态的均值和方差随时间的变化是由陀螺仪的误差模型所决定的. ...
- 好程序员web前端分享CSS Bug、CSS Hack和Filter学习笔记
为什么80%的码农都做不了架构师?>>> CSS Bug.CSS Hack和Filter学习笔记 1)CSS Bug:CSS样式在各浏览器中解析不一致的情况,或者说CSS样式在 ...
- Filter学习笔记
Filter:过滤器 1.概念: web中的过滤器:当访问服务器的资源时,过滤器可以将请求拦截下来,完成一些特殊的功能 作用: 一般用于完成通过的操作.如:登录验证.同一编码处理.敏感字符过滤-- 2 ...
- OpenCV学习笔记(三十六)——Kalman滤波做运动目标跟踪 OpenCV学习笔记(三十七)——实用函数、系统函数、宏core OpenCV学习笔记(三十八)——显示当前FPS OpenC
OpenCV学习笔记(三十六)--Kalman滤波做运动目标跟踪 kalman滤波大家都很熟悉,其基本思想就是先不考虑输入信号和观测噪声的影响,得到状态变量和输出信号的估计值,再用输出信号的估计误差加 ...
- Vue学习(watch、computed、生命周期、filter)- 学习笔记
文章目录 Vue学习(watch.computed.生命周期)- 学习笔记 watch 监听 computed 计算属性 set---动态添加属性 delete---删除 生命周期 filter 过滤 ...
- python map zip_Python学习笔记(九) map、zip和filter函数
这篇文章主要介绍 Python 中几个常用的内置函数,用好这几个函数可以让自己的代码更加 Pythonnic 哦 1.map map() 将函数 func 作用于序列 seq 的每一个元素,并返回处理 ...
- 【javaweb学习笔记】servlet-api,filter和Listener
javaweb学习笔记 1. servlet-api 1.1 servlet初始化 1.2 ServletContext和context-param 2. 什么是业务层 3. IOC 3.1 耦合/依 ...
- 四元数非正式笔记梳理_Quaternion kinematics for the error-state Kalman filter
文章目录 Introduction 0.1 为什么要用到四元数? 0.2 先说一说四元数的Hamilton和JPL流派 1 Quaternion definition and properties 1 ...
最新文章
- Foundation ActionScript 3.0.With Flash CS3 And Flex ..
- 快讯 | 清华成立AI研究院,与谷歌深度合作,张钹、姚期智、Jeff Dean坐镇
- 遗传算法在JobShop中的应用研究(part1: 绪论)
- OpenCV文字绘制支持中文显示
- SU Demos-05Sorting Traces-03susorty
- 中国水银矿石行业市场供需与战略研究报告
- @程序员,你真的会用 Unix 命令?
- stretchlim函数
- Matlab读取音频文件并进行分析
- 面向对象程序设计第三单元总结(规格系列)
- Transformations in signals and systems DSP
- Ros双线主辅同时映射一台服务器
- ios 融云 重写对话列表_iOS开发融云即时通讯集成详细步骤
- 感恩母亲节主题活动照片作品征集小程序
- zookeeper羊群效应
- Java笔试面试-JVM
- 【NOIP%你题】【线性筛】dun题解
- mktime时间的转换
- 凯文.柯恩《真爱无尽精选辑》
- 空腹时 再饿都不能吃的11种食物
热门文章
- noi.ac NOIP2018 全国热身赛 第四场 T1 tree
- spring AOP解析之xml方式详解
- 在线网摘收藏?让Google来吧!
- msf与cs互相传动
- 【数据结构与算法】之深入解析“最长有效括号”的求解思路与算法示例
- LeetCode Algorithm 剑指 Offer II 056. 二叉搜索树中两个节点之和
- 设主存容量为1MB,外存容量为400MB,计算机系统的地址寄存器有32位,那么虚拟存储器的最大容量是( )
- Struts2 ognl表达式
- 【Qt】Visual Studio编译Qt项目报Qt Version错误
- 【Tiny4412】烧写Qt文件系统到SD卡