卡尔曼滤波器原理详解

卡尔曼滤波
3
算法推导 推导最优卡尔曼增益
Kk
Pk 达到最小。
的二范数的
2
最优化K：使后验估计 k 的协方差
（换一个概念）也是使向量
ˆ x
wk.baidu.comˆk xk x
数学期望值最小化的一个过程。
ˆk | } E{| xk x
这等同于后验估计的协方差矩阵的迹最小化
ˆk ˆk | } trP P{| xk x
dtr ( BAC ) T T B C 引入常用数学公式？？ dA
T 1 T T 1 ˆ ˆ ˆ 解得： Kk PkH Sk PkH ( HPkH R)
3.6
卡尔曼滤波
3
算法推导 化简后验误差协方差公式
在卡尔曼增益等于上面导出的最优值时，计算后验协 方差的公式可以进行简化。 对于卡尔曼增益公式
nx1 真值
2.5 2.6
卡尔曼滤波
2
算法前提
定义先验估计误差的协方差为
Pk E{ek , ek } cov( ek )
T
2.7
定义 后验估计误差的协方差为
Pk E{ek , ek } cov( ek )
T
2.8
卡尔曼滤波
3
算法推导 目的描述：
在系统结构已知的情况下，给定k时刻的状态观测向 量 k，求k时刻的系统状态向量的最优估计 k ，使 得 Pk 最小。
2
卡尔曼滤波
3
算法推导
首先展开3.4式，得：
T T ˆ ˆ ˆ ˆ Pk Pk KkHPk Pk H Kk Kk T T ˆ ( HPk H R) Kk
T ˆ 记： Sk HPkH R
上面的式子可以写为：
T T T ˆ ˆ ˆ ˆ Pk Pk KkHPk PkH Kk KkSkKk
带入
ˆk x ˆk Kk ( zk Hx ˆk ) x
zk Hx k vk
ˆ k cov(xk ( x ˆk Kk ( zk Hx ˆk ))) P
再带入
ˆ k cov(xk ( x ˆk Kk ( Hxk vk Hx ˆk ))) P
卡尔曼滤波
z
ˆ x
基本思路：
1、根据
ˆ k 的递推公式 同时也产生了先验估计的协方差矩阵 P T k k 1
ˆk Ax ˆk 1 Bu k 1 x
ˆk 1 计算出k时刻的先验估计 x ˆk x
A Q
3.1
ˆ AP ˆ P
3.2
卡尔曼滤波
3
算法推导
ˆk 计算出k时刻的观测向量的估计 x ˆk ˆk Hx z ˆ k 的差，以此来修正之 3、计算实测值 zk 与估计 z ˆk ˆk ，得到后验估计 x 前的先验估计 x ˆk x ˆk Kk ( zk z ˆk ) x ˆ k 带入： x 把z ˆk x ˆk Kk ( zk Hx ˆk ) 3.3
4
算法总结 时间更新方程
ˆk Ax ˆk 1 Bu k 1 x T ˆ ˆ Pk APk 1 A Q
4.1 4.2 4.3 4.4 4.5
测量更新方程
T T 1 ˆ ˆ Kk PkH ( HPkH R) ˆk x ˆk K ( zk Hx ˆk ) x ˆ k ( I KkH ) P ˆk P
ˆ k 趋向于零时，根据式3.2可知，Q=0 反之当 P
nx1
易知，此时系统完全抛弃测量值，取先验估计值 卡尔曼滤波
4
算法总结 算法描述
卡尔曼滤波器用反馈控制的方法估计过程状态。估计 过程某一时刻的状态，然后以（含噪声）测量值得方 式获得反馈，因此卡尔曼滤波器可以分为两个部分： 1、时间更新方程 负责向前推算当前状态向量和误差协方差估计的值， 为下一个时间状态构成先验估计。 2、测量更新方程 负责将先验估计和新的测量变量结合已构成改进后的 后验估计。 卡尔曼滤波
A· AT
Q
ˆk P
ˆk P
卡尔曼滤波
5
算法扩展 滤波器参数调整
在卡尔曼滤波器实际实现时，测量噪声R一般可以观 测得到，是滤波器的已知条件。观测测量噪声协方差 R可以通过离线试验获取。 而过程激励噪声协方差Q值比较难以确定，因为我们 无法直接观测到过程状态 xk 。有时候可以通过Q的 手动选择，人为的为过程“注入”一个足够的不确定 因素，来产生一个可以接受的结果。 也可以使用一个离线模拟滤波器与在线滤波器进行实 时对比的方式来实时优化滤波器的参数（Q和R）。 卡尔曼滤波
2
算法前提 随机离散系统模型
定义随机离散时间过程的状态向量 程由以下离散随机差分方程描述：
xk R ，该过
n
x k Ax k 1 Bu k 1 wk 1 nx1 nxn nxn nx1 nx1
假设系统满足可观性要求，定义观测向量 得到观测方程：
2.1
m
zk R ，
3
算法推导
整理测量误差向量，得：
ˆ k cov(( ˆk ) Kkvk ) P I KkH )(xk x
因为噪声项与其他项不相关，协方差=0，所以有：
ˆ k cov(( ˆk )) cov(Kkvk ) P I KkH )(xk x
利用协方差矩阵性质，提出常数矩阵，得：
趋向于零时，有：
Pk 0
lim K k 0 ˆ
卡尔曼滤波
3
算法推导
意义：决定了最优估计组成比例的“调节器” 当 R 趋向于零时，有： lim Kk
R 0
H
1
测量噪声 V=0
mx1
此时3.3式改为
1 1 ˆ ˆ ˆ xk xk H ( zk Hxk ) H zk
系统表现为完全取测量值作为状态的后验估计值，而 系统的先验状态估计完全被抛弃。
z k Hx k vk mx1 mxn mx1
2.2
随机信号 k 和 k 分别表示过程激励噪声和观测噪 声，并假设它们是相互独立并满足正态分布的白噪声。 卡尔曼滤波
w
v
2
算法前提
即有： 过程激励噪声 观测噪声
wk ~ N (0, Q)
vk ~ N (0, R)
2.3 2.4
实际过程中过程激励噪声协方差矩阵Q和观测噪声协 方差矩阵R可能随着每次迭代计算而变化，但在这里 我们假设它们为常数。 另外，状态转移矩阵A，输入矩阵B和测量方程2.2中 的矩阵H，在实际过程中都可能随时间变化而变化， 但在这儿假设为常数。 卡尔曼滤波
3.7
3
算法推导 卡尔曼增益的物理意义
T ˆ Pk H Kk T ˆ HPk H R
ˆ k 与过程激励噪声的协方差 其中：H矩阵为常量；P 矩阵Q有关；R为测量噪声的协方差矩阵。
取值范围： Kk [0, H
1
当 R 趋向于零时，有：
当
] 1 lim Kk H
R 0
ˆk P
卡尔曼滤波器原理
林斌 P201109026
内容提纲
1
发展概述
算法前提 算法推导 算法总结 算法扩展
2
3
4
5
卡尔曼滤波
1
发展概述
工程背景：1960s
航空航天工程突飞猛进 电子计算机又方兴未艾
Rudolf (Rudy) Emil Kálmán （1930～ ）
正式提出：
《一种关于线性过滤和预测难题的新方法》1960 "A new approach to linear filtering and prediction problems " ， 1960
2、根据先验估计
这里的权重系数也成为卡尔曼增益。 至此卡尔曼滤波计算原型公式基本结束， 下面要确定出最优系数K和在最优系数下 的后验估计误差的协方差矩阵的递推方 法 权重 残差
卡尔曼滤波
3
算法推导
ˆk 推导后验协方差矩阵 P ˆ k 开始推导如下： 按照定义，我们从误差协方差 P
ˆ k cov(xk x ˆk ) P
T ˆ ˆk )(I KkH ) Pk ( I KkH ) cov(xk x
Kk cov(vk ) Kk
T
卡尔曼滤波
3
算法推导
如果记
ˆk ˆk ) P cov(xk x
，则有：
T T ˆ ˆ Pk ( I KkH )Pk ( I KkH ) KkRKk
3.4
卡尔曼滤波
1
发展概述 优势：
1、采用物理意义较为直观的时域状态空间 2、仅需要前后两步的数据，数据存储量较小 3、使用比较简单的递推算法，便于在计算机上实现 4、不仅适用于平稳过程，还可以推广到非平稳随机 过程的情况
发展：
卡尔曼滤波器已成为推广研究和应用的主题，尤其是 在自主或协助导航领域。
卡尔曼滤波
T 1 ˆ Kk PkH Sk
在卡尔曼增益公式两侧同时右乘
得：
SkKk
T
KkSkKk
T
T T ˆ PkH Kk
把上式带入3.5式，可以消去后面的两项，得：
ˆk P ˆ k KkHP ˆk P
卡尔曼滤波
3
算法推导
整理，得：
ˆ k ( I KkH ) P ˆk P
这个公式的计算比较简单，所以实际中总是使用这个 公式，但是需注意这公式仅在使用最优卡尔曼增益的 时候它才成立。如果算术精度总是很低而导致数值稳 定性出现问题，或者特意使用非最优卡尔曼增益，那 么就不能使用这个简化；必须使用3.5式表示的后验误 差协方差公式。
卡尔曼滤波
4
算法总结 算法框图
1、状态估计向量的运算流程图
u
时间更新（预测） k 1 B A
ˆk 1 x

测量更新（校正）
ˆk x
H
ˆk z

K
ˆk x
zk
卡尔曼滤波
4
算法总结 算法框图
2、误差协方差的运算流程图 时间更新（预测） 测量更新（修正） I -KkH
ˆk 1 P
2
算法前提 先验估计和后验估计，及其误差
定义 估计值，称为先验估计。
n n ˆ xk R 为根据上一次迭代计算结果而产生的
ˆk R 为根据当前计算结果而产生的估计值， 定义 x 称为后验估计。
定义先验估计误差为 定义后验估计误差为
ˆk ek xk x ˆk ek xk x
nx1 真值
3.5
卡尔曼滤波
3
算法推导
把
ˆ k ) 对 Kk tr( P
ˆk) 求导，并令导数=0，则可以得到 tr ( P
Kk
的值。
取最小值时的最优化
ˆk) dtr( P ˆ k )T P ˆ kH T ( SkKk T )T ( KkSk ) ( HP dKk ˆ k H T 2 KkSk 0 2 P