Tutorial of kalman filter

姓名：周圣砚
邮箱：zhousy@mit.edu
zhoushengyan2006@hotmail.com
单位：北京理工大学智能车辆研究所

Why Kalman?y
Kalman理论基础Kalman理论基础
Kalman滤波器介绍Kalman滤波器介绍
Kalman滤波器应用实例Kalman滤波器应用实例
参考文献参考文献

 鲁道夫·卡尔曼（Rudolf Emil Kalman, 1930年5
月－），匈牙利数学家，1930年出生于匈牙利
首都布达佩斯 1953 1954年于麻省理工学院首都布达佩斯。1953-1954年于麻省理工学院
分别获得电机工程学士及硕士学位。1957年于
哥伦比亚大学获得博士学位曾任职于斯坦福哥伦比亚大学获得博士学位。曾任职于斯坦福
大学，佛罗里达大学，瑞士苏黎世联邦理工学
院院
 Kalman filter invented in 1960 by R. E. Kalman

落 自由落体系统
sensorx x

sensor
sensory y


观测数据：

落 自由落体系统
sensorx x

sensor
sensory y


观测数据：
x v
y at



系统方程：
y at

 观测数据：
imua a

imu
sensorv v




 观测数据：
imua a

imu
sensorv v



 可用系统方程：
1t t ta v v  
min maxta a a 
1t tv v 

 Kalman滤波器的产生，是为了摆脱那些对没有
任何物理意义的观测数据本身进行的信号处理，
通过分析系统属性，建立精确的系统数学模型，
模型来辅测从提高对系统估计精用模型来辅助预测，从而提高对系统的估计精
度。Kalman正是一种连接（或者说融合）系统
模型和观测数据的纽带有机的结合了两者的模型和观测数据的纽带。有机的结合了两者的
优势。
 The Kalman filter uses a system's dynamics model (i e physical The Kalman filter uses a system s dynamics model (i.e., physical
laws of motion), known control inputs to that system, and
measurements (such as from sensors) to form an estimate of the
system's varying quantities (its state) that is better than thesystem s varying quantities (its state) that is better than the
estimate obtained by using any one measurement alone. As such,
it is a common sensor fusion algorithm.

 我们想知道NASA太空舱的实际位置:
 我们有什么？
 我们有一组关于太空舱位置的观测值
 我们有一个能够描述系统位置和状态的动态模
ty
我
型 tx
我们观测到，但我们实际上想要ty tx

 高斯噪声
 先验概率/先验概率密度（Prior probability）
 后验概率/后验概率密度（Posterior probability）
 期望，协方差(covariance)

 什么叫做高斯噪声
零均值
正太分布

事 定义：在没有任何“信息”到来之前，所有事
件集在事件空间里发生的概率。
 数学表达：
 例子1：投硬币
( ),P x x事件空间
 例子2：掷骰子
0,1, ( 0) 50%, ( 1) 50%x P x P x    
掷
1,2,3,4,5,6; ( 1,2,3,4,5,6) 1/ 6.x P x  

事 定义：在给定一定的“信息”之后，发生某事
件的概率。
 数学表达：假设已知信息是z，事件为x
( | ), zP x z x 事件空间，条件空间
 例子1：投硬币
假设“信息”z：此次投的不是正面（1）
( | ),
息是
( 1| ) 0, ( 0 | ) 100P x z P x z   
 假设“信息”z：此次掷的不是1
( 1| ) 0, ( 2,3,4,5,6 | ) 1/ 5P x z P x z   

 期望：
 方差：
( ) ( ) , ( ) i iE x xf x dx E x x P  
2
([ ( )] )E X E X
 协方差： (( ( ))( ( )))E X E X Y E Y 
( ( ) ( ) ( ) ( ))E XY XE Y YE X E X E Y   
( ) ( ( )) ( ( )) ( ( ) ( ))E XY E XE Y E YE X E E X E Y   
( ) ( ) ( ) ( ) ( ) ( ) ( )E XY E X E Y E Y E X E X E Y   ( ) ( ) ( ) ( ) ( ) ( ) ( )E XY E X E Y E Y E X E X E Y   
( ) ( ) ( )E XY E X E Y 

态 系统方程（状态方程）：
1 1 1k k k kx Ax Bu w    
 举例1：自由落体物体速度为，信号采集频率
为100Hz，该系统状态方程为
kx
 举例2：匀速运动小车，速度为c(m/s)，设位移
1 1/100k k kx x g w   
举车度为 ( ) 位移
为，位移采集频率为1Hz，该系统状态方程
为
kx
1 1k k kx x c w   

 测量方程：
k k kz Hx v 
 举例1：自由落体物体速度为，传感器为风力
测速计，该系统测量方程为
kx
 举例2：匀速运动小车，传感器选用GPS传感器，
k k kz x v 
举车传选传
该系统测量方程为tx
1k k kx x v  

随机变量和分别代表过程噪声和测量噪声，
它们是独立不相关且正太分布的白噪声。
kw kv
它们是独立不相关且正太分布的白噪声。
其中Q和R分别是过程噪声和测量噪声的协方差
( ) (0, ), ( ) (0, ).p w N Q p v N R 
其中Q和R分别是过程噪声和测量噪声的协方差
矩阵。
完整的Kalman滤波器表达式：完整的Kalman滤波器表达式：
1 1 1k k k kx Ax Bu w     ( ) (0, )p w N Q
k k kz Hx v  ( ) (0, )p v N R
小提示：在实际的系统中，A，B，H，Q和R可能在系统运行过程
中会发生变化
比如：惯导数据比如：惯导数据

贝叶斯原理。
( | ) ( )
( | )k kP x z
( | ) ( )
( | )
( )
k k k
k k
k
p z x p x
P x z
p z

( | ) ( )
1
1
( | ) ( | )
( | )
k k k k
k k
p z x p x z
p z z



1
1
( , | ) ( )
( , )
k k k k
k k
p z z x p x
p z z



( | ) ( | ) ( )1 1
1 1
( | , ) ( | ) ( )
( | ) ( )
k k k k k k
k k k
p z z x p z x p x
p z z p z
 
 

1 1 1
1 1
( | , ) ( | ) ( ) ( )
( | ) ( ) ( )
k k k k k k k
k k k k
p z z x p x z p z p x
p z z p z p x
  
 

1
1
( | ) ( | )
( | )
k k k k
k k
p z x p x z
p z z



(1)
1( | )k kp 

贝叶斯原理。 ( | )k kP x z
1
1
( | ) ( | )
( | )
k k k k
k k
p z x p x z
p z z



状态方程
X(k)=AX(k-1)

1
1
( | ) ( | )
( | )
k k k k
k k
p z x p x z
p z z



状态方程
X(k)=AX(k-1)
测量方程
Z(k)=HX(k)
1 1( | ) ( , )k k kP x x N Ax Q 1 1( | ) ( , )k k k Q

1
1
( | ) ( | )
( | )
k k k k
k k
p z x p x z
p z z



测量方程
Z(k)=HX(k)
状态方程
X(k)=AX(k-1)
X(k)
1 1( | ) ( , )k k kP x x N Ax Q  ( | ) ( , )k k kP z x N Hx R(2) (3)1 1( | ) ( , )k k k Q ( | ) ( , )k k k
1 1 1( | ) ( | )k k k kP x x P x z dx   1( | )k kP x z 
( )
(4)

假设是给定上一步给出的先验状态估计。
假设是给定了当前测量值的后验状态估计
ˆ n
kx
 1kx 
ˆ n
假设是给定了当前测量值的后验状态估计。
我们定义先验和后验估计误差为：
ˆ n
kx  kz
ˆk k ke x x 
 
ˆe x x 
(5)
(6)
那么，先验估计误差的协方差矩阵为：
k k ke x x (6)
那计阵
( )T
k k kP E e e  
 (7)
后验估计误差的协方差矩阵为：
T (8)( )T
k k kP E e e (8)

在推导Kalman滤波器方程时，我们的目标是找到
一个能够计算后验状态估计方程，并且该方程ˆx个能够计算后验状态估计方程，并且该方程
是先验估计和加权的测量余量的线性组合。
kx
ˆkx
K被叫做gain，通过最小化后验误差协方差(8)得
ˆ ˆ ˆ( )k k k k kx x K z Hx 
   (9)
叫 g ( )
到K ([1,2,3])
T
kP H
K

k
k T
k
K
HP H R

 (10)
测量余量：实际测量值与预测测量值之差。kz ˆkHx
k k
ˆ( )k kz Hx


Time update equations
Measurement update equations

设个车在做匀速速度为5( / ) 假设一个小车在做匀速运动，加速度为5(m/s)，
小车上装有速度传感器，采样频率为10Hz，传
感器测量误差为高斯白噪声。试对小车的运动感器测量误差为高斯白噪声。试对小车的运动
速度进行估计。
 设k时刻时的速度为，则系统的状态方程为：
1 10.5k k kx x w   
kx
 测量方程为：
1 1k k k 
k k kz x v 
 参阅demo: LinearKF1.m
k k kz x v

 假设车辆上装有速度传感器和加速度传感器，
感采率为1H 感两传感器的采样频率为1Hz，试通过传感器数
据对车辆的速度进行估计。
态 设，系统的状态方程为：
111 1 k
w
X X

  
   
[ , ]'k k kX v a
 测量方程为：
1
1
20 1 k
k kX X
w 
    
    
11
2
1 0
0 1
k
k k
v
Z X
v

  
    
    
 参阅demo: LinearKF2 m
12k    

题目假设有个垃圾GPS 干扰十分强烈幸题目：假设有一个垃圾GPS，干扰十分强烈。幸
运的是，我们买了一个惯导和车速传感器，买
来以后得知惯导性能也很垃圾。试通过融合来以后得知惯导性能也很垃圾。试通过融合
GPS，惯导和车速传感器三个传感器信息，对
机器人位置进行估计（采样频率都为1Hz）机器位置进行估计（采样频率都为）
条件：为了简化题目难度，我们假设机器人在水
平路面上沿直线运动，惯导输出的是纵向加速平路面上沿直线运动，惯导输出的是纵向加速
度值。
数据参阅： LinearKF3 m数据参阅： LinearKF3.m

M b k P S 1979 S h i M d l E i i d Maybeck, Peter S. 1979. Stochastic Models, Estimation, and
Control, Volume 1, Academic Press, Inc.
 Brown, R. G. and P. Y. C. Hwang. 1992. Introduction to Random
Signals and Applied Kalman Filtering, Second Edition, John Wiley
& Sons, Inc.
 Grewal, Mohinder S., and Angus P. Andrews (1993). Kalman, , g ( )
Filtering Theory and Practice. Upper Saddle River, NJ USA,
Prentice Hall.
 Greg Welch and Gary Bishop. An Introduction to the Kalman Greg Welch and Gary Bishop. An Introduction to the Kalman
Filter

Tutorial of kalman filter

Recommended

Recommended

More Related Content

What's hot

What's hot (20)

Similar to Tutorial of kalman filter

Similar to Tutorial of kalman filter (11)

Tutorial of kalman filter