[转]python起步之卡尔曼滤波
关于卡尔曼滤波的理论这里不打算讲了,就是那个5个基本的公式,这里直接给出公式:
公式1:X(k|k-1) = AX(k-1 | k-1) + BU(k) + W(k)
公式2:P(k|k-1) = AP(k-1|k-1)A' + Q(k)
公式3:X(k|k) = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)
公式4:Kg(k) = P(k|k-1)H'/{HP(k|k-1)H' + R}
//卡尔曼增益
公式5:P(k|k) = (1- Kg(k) H) P(k|k-1)
另外,Z(k) = HX(k) + V
,Z是测量值,X是系统值,W是过程噪声,V是测量噪声,H是测量矩阵,A是转移矩阵,Q是W的协方差,R是V的协方差,X(k|k-1)是估计值;X(k|k)是X(k|k-1)的最优估计值,即滤波估计值;P(k|k-1)是估计值误差方差矩阵,P(k|k)是滤波误差方差矩阵。
下面给出Python版本的卡尔曼滤波小程序,这里设置A=1,H=1,BU=0,W=0
:
# -*- 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的情况
# intial parameters
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
# allocate space for arrays
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):
# time update
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
# measurement update
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()
结果:
[转]python起步之卡尔曼滤波的更多相关文章
- Python起步学习
Python起步学习 案例1:程序输入输出 案例2:判断合法用户 安全3:编写判断成绩的程序 1 案例1:程序输入输出 1.1 问题 编写login.py脚本,实现以下目标: 提示用户输入用户名 将用 ...
- Python起步(2)
单行注释:#多行注释:'''或""" 一条语句写在一行之内,不需要分号分隔两条语句在同一行,中间分号隔开缩进语句块中只有一条语句,可以直接写在“:”之后使用“\”进行续行 ...
- WINDOWS+NGINX+DJANGO+FLUP+PYTHON起步~
参考的文档是 http://blog.163.com/sky20081816@126/blog/static/1647610232010824262695/ 但在实操时,作了更改之后才生效,就是#in ...
- Python起步
最近研究系统自动化测试想起了一年前有学习Python的想法,借此机会准备抽时间好好学学.为方便以后学习和查询特写以下博客! Python基础 1. Python数据结构 (1)Python字符串 (2 ...
- 【python学习-2】python起步必备
1.python缩进 python 缩进是tab,还是空格呢?都可以,可以是一个tab,也可以是4个空格,但是最重要的是整个python脚本的缩进必须统一,否则会报错. 2.代码注释 python注释 ...
- Python 起步 多版本共存配置
上次我选择的是py2.x,如果我要再装一个py3.x呢 我们去设置环境变量,然后去命令行输入python,这里我故意把环境变量放在第一行,貌似换成3.7了 我们把2.7的放在3.7的前面呢?又换回去了 ...
- Python 起步 环境配置
1:https://www.python.org/ 首先进入这个网址,选择自己喜欢的版本 2:嘛,我以前装的是2.7,把下载好的安装一下就行 3:我的电脑Python的安装路径C:\Progra ...
- python起步--windows系统下安装python解释器和PyCharm
参考教程: 1)https://www.runoob.com/w3cnote/pycharm-windows-install.html 2)https://blog.csdn.net/c_shell_ ...
- 第一章Python起步
1.1搭建编程环境 编程环境的正确搭建很重要,一定要参考先搭配好环境变量,不然用着会很麻烦,在这里推荐使用工具pycharm,亿图图示画流程图,一定要正确安装,搭配好环境变量,后面要添加很多模块,前期 ...
随机推荐
- Android ListView高度自适应和ScrollView冲突解决
在ScrollView中嵌套使用ListView,ListView只会显示一行到两行的数据.起初我以为是样式的问题,一直在对XML文件的样式进行尝试性设置,但始终得不到想要的效果.后来在网上查了查,S ...
- Java中中文拼音的排序问题
最近做一个手机数据同步的应用开发,需要提供地址簿信息按照姓名的拼音次序进行排序.但仔细考察Java提供的Collator之后,发现其中文拼音排序存在严重的问题.Java提供Collator来支持不同语 ...
- Spring 操作数据库
试了一下spring的JdbcTemplate觉得很好用.首先增加一个连接到mysql数据库的dataSource <bean id="dataSource2" class= ...
- powerdesigner连接数据库 导出数据 生成PDM文件 傻瓜试教程
也可下载文档:http://download.csdn.net/detail/shutingwang/6378665
- POJ 1808 Quadratic Residues(平方剩余相关)
题目链接:http://poj.org/problem?id=1808 题意:如下.对于素数p,若存在x使得x^2%p=a,则其值为1.否则为-1.现在给出a.p,计算其值. 思路: 若a为正数则利用 ...
- [POJ1753]Flip Game(开关问题,枚举)
题目链接:http://poj.org/problem?id=1753 和上一个题一样,将初始状态存成01矩阵,就可以用位运算优化了.黑色白色各来一遍 /* ━━━━━┒ギリギリ♂ eye! ┓┏┓┏ ...
- trackr: An AngularJS app with a Java 8 backend – Part I
该系列文章来自techdev 我想分享在techdev公司开发的项目-trackr-的一些最新的见解.trackr是一个用来跟踪我们的工作时间,创建报告和管理请假的web应用程序.做这个程序的目的有两 ...
- 学习Hadoop不错的系列文章
1)Hadoop学习总结 (1)HDFS简介 (2)HDFS读写过程解析 (3)Map-Reduce入门 (4)Map-Reduce的过程解析 (5)Hadoop的运行痕迹 (6)Apache Had ...
- HDU4888 Redraw Beautiful Drawings(2014 Multi-University Training Contest 3)
Redraw Beautiful Drawings Time Limit: 3000/1500 MS (Java/Others) Memory Limit: 65536/65536 K (Jav ...
- Leap Motion 开发笔记
Leap Motion 体系架构 Leap Motion支持所有主流操作系统,leap motion在Windows系统下是作为一个服务运行的,在Mac和Linux是后台守护进程.软件通过USB线连接 ...