math: 卡尔曼滤波算法原理以及python实例
基本动态系统模型
卡尔曼滤波建立在线性代数和隐马尔可夫模型(hidden Markov model)上。其基本动态系统可以用一个马尔可夫链表示,该马尔可夫链建立在一个被高斯噪声(即正态分布的噪声)干扰的线性算子上的。系统的状态可以用一个元素为实数的向量表示。随着离散时间的每一个增加,这个线性算子就会作用在当前状态上,产生一个新的状态,并也会带入一些噪声,同时系统的一些已知的控制器的控制信息也会被加入。同时,另一个受噪声干扰的线性算子产生出这些隐含状态的可见输出。
为了从一系列有噪声的观察数据中用卡尔曼滤波器估计出被观察过程的内部状态,我们必须把这个过程在卡尔曼滤波的框架下建立模型。也就是说对于每一步k,定义矩阵Fk, Hk, Qk,Rk,有时也需要定义Bk,如下。
卡尔曼滤波模型假设k时刻的真实状态是从(k − 1)时刻的状态演化而来,符合下式:
其中
- Fk是作用在xk−1上的状态变换模型(/矩阵/矢量)。
- Bk是作用在控制器向量uk上的输入-控制模型。
- wk是过程噪声,并假定其符合均值为零,协方差矩阵为Qk的多元正态分布。
时刻k,对真实状态xk的一个测量zk满足下式:
其中Hk是观测模型,它把真实状态空间映射成观测空间,vk是观测噪声,其均值为零,协方差矩阵为Rk,且服从正态分布。
初始状态以及每一时刻的噪声{x0, w1, ..., wk, v1 ... vk}都认为是互相独立的。
实际上,很多真实世界的动态系统都并不确切的符合这个模型;但是由于卡尔曼滤波器被设计在有噪声的情况下工作,一个近似的符合已经可以使这个滤波器非常有用了。更多其它更复杂的卡尔曼滤波器的变种,在下边讨论中有描述。
卡尔曼滤波器
卡尔曼滤波是一种递归的估计,即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值,因此不需要记录观测或者估计的历史信息。卡尔曼滤波器与大多数滤波器不同之處,在於它是一种纯粹的时域滤波器,它不需要像低通滤波器等频域滤波器那样,需要在频域设计再转换到时域实现。
卡尔曼滤波器的状态由以下两个变量表示:
- ,在时刻k的状态的估计;
- ,误差相关矩阵,度量估计值的精确程度。
卡尔曼滤波器的操作包括两个阶段:预测与更新。在预测阶段,滤波器使用上一状态的估计,做出对当前状态的估计。在更新阶段,滤波器利用对当前状态的观测值优化在预测阶段获得的预测值,以获得一个更精确的新估计值。
预测
- (预测状态)
- (预测估计协方差矩阵)
更新
首先要算出以下三个量:
- (测量余量,measurement residual)
- (测量余量协方差)
- (最优卡尔曼增益)
然后用它们来更新滤波器变量x与P:
- (更新的状态估计)
- (更新的协方差估计)
使用上述公式计算仅在最优卡尔曼增益的时候有效。使用其他增益的话,公式要复杂一些,请参见推导。
不变量(Invariant)
如果模型准确,而且与的值准确的反映了最初状态的分布,那么以下不变量就保持不变:所有估计的误差均值为零
且协方差矩阵准确的反映了估计的协方差:
请注意,其中表示的期望值, 。
实例
考虑在无摩擦的、无限长的直轨道上的一辆车。该车最初停在位置0处,但时不时受到随机的冲击。我们每隔△t秒即测量车的位置,但是这个测量是非精确的;我们想建立一个关于其位置以及速度的模型。我们来看如何推导出这个模型以及如何从这个模型得到卡尔曼滤波器。
因为车上无动力,所以我们可以忽略掉Bk和uk。由于F、H、R和Q是常数,所以时间下标可以去掉。
车的位置以及速度(或者更加一般的,一个粒子的运动状态)可以被线性状态空间描述如下:
其中是速度,也就是位置对于时间的导数。
我们假设在(k-1)时刻与k时刻之间,车受到ak的加速度,其符合均值为0,标准差为σa的正态分布。根据牛顿运动定律,我们可以推出
其中
且
我们可以发现
- (因为σa是一个标量)。
在每一时刻,我们对其位置进行测量,测量受到噪声干扰。我们假设噪声服从正态分布,均值为0,标准差为σz。
其中
且
如果我们知道足够精确的车最初的位置,那么我们可以初始化
并且,我们告诉滤波器我们知道确切的初始位置,我们给出一个协方差矩阵:
如果我們不确切的知道最初的位置与速度,那么协方差矩阵可以初始化为一个对角线元素是B的矩阵,B取一个合适的比较大的数。
此时,与使用模型中已有信息相比,滤波器更倾向于使用初次测量值的信息。
推导后验协方差矩阵
按照上边的定义,我们从误差协方差开始推导如下:
代入
再代入
与
整理误差向量,得
因为测量误差vk与其他项是非相关的,因此有
利用协方差矩阵的性质,此式可以写作
使用不变量Pk|k-1以及Rk的定义这一项可以写作 : 这一公式对于任何卡尔曼增益Kk都成立。如果Kk是最优卡尔曼增益,则进一步简化,請見下文。
最优卡尔曼增益的推导
卡尔曼滤波器是一个最小均方误差估计器,后验状态误差估计(英文:a posteriori state estimate)是
我们最小化这个矢量幅度平方的期望值,,这等同于最小化后验估计协方差矩阵Pk|k的迹(trace)。将上面方程中的项展开、抵消,得到:
当矩阵导数是0的时候得到Pk|k的迹(trace)的最小值:
此处须用到一個常用的式子,如下:
从这个方程解出卡尔曼增益Kk:
这个增益称为最优卡尔曼增益,在使用时得到最小均方误差。
后验误差协方差公式的化简
在卡尔曼增益等于上面导出的最优值时,计算后验协方差的公式可以进行简化。在卡尔曼增益公式两侧的右边都乘以SkKkT得到
根据上面后验误差协方差展开公式,
最后两项可以抵消,得到
- .
这个公式的计算比较简单,所以实际中总是使用这个公式,但是需注意这公式仅在使用最优卡尔曼增益的时候它才成立。如果算术精度总是很低而导致数值稳定性出现问题,或者特意使用非最优卡尔曼增益,那么就不能使用这个简化;必须使用上面导出的后验误差协方差公式。
Python代码
- # -*- coding=utf-8 -*-
- # Kalman filter example demo in Python
- # A Python implementation of the example given in pages 11-15 of "An
- # Introduction to the Kalman Filter" by Greg Welch and Gary Bishop,
- # University of North Carolina at Chapel Hill, Department of Computer
- # Science, TR 95-041,
- # http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
- # by Andrew D. Straw
- #coding:utf-8
- import numpy
- import pylab
- #这里是假设A=1,H=1的情况
- # 参数初始化
- n_iter = 50
- sz = (n_iter,) # size of array
- x = -0.37727 # truth value (typo in example at top of p. 13 calls this z)真实值
- z = numpy.random.normal(x,0.1,size=sz) # observations (normal about x, sigma=0.1)观测值
- Q = 1e-5 # process variance
- # 分配数组空间
- xhat=numpy.zeros(sz) # a posteri estimate of x 滤波估计值
- P=numpy.zeros(sz) # a posteri error estimate滤波估计协方差矩阵
- xhatminus=numpy.zeros(sz) # a priori estimate of x 估计值
- Pminus=numpy.zeros(sz) # a priori error estimate估计协方差矩阵
- K=numpy.zeros(sz) # gain or blending factor卡尔曼增益
- R = 0.1**2 # estimate of measurement variance, change to see effect
- # intial guesses
- xhat[0] = 0.0
- P[0] = 1.0
- for k in range(1,n_iter):
- # 预测
- xhatminus[k] = xhat[k-1] #X(k|k-1) = AX(k-1|k-1) + BU(k) + W(k),A=1,BU(k) = 0
- Pminus[k] = P[k-1]+Q #P(k|k-1) = AP(k-1|k-1)A' + Q(k) ,A=1
- # 更新
- K[k] = Pminus[k]/( Pminus[k]+R ) #Kg(k)=P(k|k-1)H'/[HP(k|k-1)H' + R],H=1
- xhat[k] = xhatminus[k]+K[k]*(z[k]-xhatminus[k]) #X(k|k) = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)], H=1
- P[k] = (1-K[k])*Pminus[k] #P(k|k) = (1 - Kg(k)H)P(k|k-1), H=1
- pylab.figure()
- pylab.plot(z,'k+',label='noisy measurements') #观测值
- pylab.plot(xhat,'b-',label='a posteri estimate') #滤波估计值
- pylab.axhline(x,color='g',label='truth value') #真实值
- pylab.legend()
- pylab.xlabel('Iteration')
- pylab.ylabel('Voltage')
- pylab.figure()
- valid_iter = range(1,n_iter) # Pminus not valid at step 0
- pylab.plot(valid_iter,Pminus[valid_iter],label='a priori error estimate')
- pylab.xlabel('Iteration')
- pylab.ylabel('$(Voltage)^2$')
- pylab.setp(pylab.gca(),'ylim',[0,.01])
- pylab.show()
运行结果
math: 卡尔曼滤波算法原理以及python实例相关推荐
- 卡尔曼滤波算法原理(KF,EKF,AKF,UKF)
卡尔曼滤波算法原理(KF,EKF,AKF,UKF) 主要是KF.EKF.UKF算法公式推导,直接看公式会比较枯燥,建议推导一下. 新增文章卡尔曼运动模型公式推导CTRV+CTRA,主要是EKF的CTR ...
- 手把手教你EMD算法原理与Python实现(更新)
Rose今天主要介绍一下EMD算法原理与Python实现.关于EMD算法之前介绍过<EMD算法之Hilbert-Huang Transform原理详解和案例分析>, SSVEP信号中含有自 ...
- Adaboost算法原理分析和实例+代码(简明易懂)
Adaboost算法原理分析和实例+代码(简明易懂) [尊重原创,转载请注明出处] http://blog.csdn.net/guyuealian/article/details/70995333 ...
- python kmeans聚类 对二维坐标点聚类_Kmeans均值聚类算法原理以及Python如何实现
第一步.随机生成质心 由于这是一个无监督学习的算法,因此我们首先在一个二维的坐标轴下随机给定一堆点,并随即给定两个质心,我们这个算法的目的就是将这一堆点根据它们自身的坐标特征分为两类,因此选取了两个质 ...
- 匈牙利算法原理与Python实现
匈牙利算法原理与Python实现 今天学习一个新的算法-匈牙利算法,用于聚类结果分析,先用图表示我当前遇到的问题: 这两列值是我用不同算法得到的聚类结果,从肉眼可以看出第一列聚类为0的结果在第二列中对 ...
- Apriori 算法原理以及python实现详解
Apriori 算法原理以及python实现 Apriori算法是第一个关联规则挖掘算法,也是最经典的算法.它利用逐层搜索的迭代方法找出数据库中项集的关系,以形成规则,其过程由连接(类矩阵运算)与 ...
- LM(Levenberg–Marquardt)算法原理及其python自定义实现
LM算法原理及其python自定义实现 LM(Levenberg–Marquardt)算法原理 LM算法python实现 实现步骤: 代码: 运行结果: LM(Levenberg–Marquardt) ...
- PageRank算法原理与Python实现
本文转载自https://blog.csdn.net/ten_sory/article/details/80927738 PageRank算法原理与Python实现 PageRank算法,即网页排名算 ...
- 统计学习方法笔记(一)-k近邻算法原理及python实现
k近邻法 k近邻算法 算法原理 距离度量 距离度量python实现 k近邻算法实现 案例地址 k近邻算法 kkk近邻法(kkk-NN)是一种基本分类和回归方法. 算法原理 输入:训练集 T={(x1, ...
最新文章
- maven jar包冲突常见报错及解决方法
- ImageMagick之PDF转换成图片(image)
- Crash常见异常总结
- 博士申请 | ​英属哥伦比亚大学李霄霄助理教授招收全奖博士生、硕士生
- C语言矩阵N*N旋转的算法(附完整源码)
- 数据库设计与开发概述
- 给刚入行的存储工程师10+1点建议
- 理论 | 当 Spring Boot 遇上了消息队列......
- Spring Boot(一)入门篇
- GitHub 日收 12,000 星,微软新命令行工具引爆程序员圈!
- PHP与Javascript的混合测试
- 在现有计算机内安装另一个硬盘的原因,您有一台运行Windows7的计算机。您在计算机中安装第二个内部硬盘驱动器。您尝试创建一个系统映像,.. - 上学吧找答案...
- 在阿里云上试用 Knative 1
- 爱希ISee人体感应器,雷达人体存在探测应用,毫米波雷达技术方案
- Redis 实战场景
- 中国程序员容易发错音的单词「GitHub 热点速览 v.22.23」
- 英语后置定语语法归纳
- 实现12306全自动下单功能(Python+PyCharm附:主要代码)
- Excel2010无法保存提示检测到错误
- Cordova 拨打电话_Cordova 项目开启拨打电话
热门文章
- JavaScript时间换算单位
- 使用Vivado生成ADI的ADC例程(以AD9680为例)
- NLP从入门到实战(一)
- LCD Keypad Shield
- charles抓包一直返回unkown的解决方法
- 人工智能里的数学修炼 | 隐马尔可夫模型:基于EM的鲍姆-韦尔奇算法求解模型参数
- iOS安全攻防(二十三):Objective-C代码混淆
- Windows10设置任务栏透明化
- sqlmap总出现missing a mandatory option的解决方法
- 调试时出现:undefined Expecting 'EOF','}',',',']', got STRING以下错误的解决方法