卡尔曼滤波器原理详解

mx1
w v 随机信号 k和 k 分别表示过程激励噪声和观测噪
声，并假设它们是相互独立并满足正态分布的白噪声。
卡尔曼滤波
2 算法前提
即有：
过程激励噪声 wk~N(0,Q)
观测噪声
vk ~N(0,R)
2.3
2.4
实际过程中过程激励噪声协方差矩阵Q和观测噪声协 方差矩阵R可能随着每次迭代计算而变化，但在这里 我们假设它们为常数。
P ˆkAP ˆk1ATQ
3.2
卡尔曼滤波
3 算法推导
xˆ 2、根据先验估计 k 计算出k时刻的观测向量的估计
zˆk Hxˆk
z zˆ 3、计算实测值 k 与估计 k 的差，以此来修正之
xˆ xˆ 前的先验估计 k ，得到后验估计 k
x ˆkx ˆkK k(zkz ˆk)
把 zˆk 带入： x ˆkx ˆkK k(zkH x ˆk)3.3
如果记 coxvk(xˆk)P ˆk ，则有： P ˆk (I K k H )P ˆk (I K k H )T K k Rk TK
3.4
卡尔曼滤波
3 算法推导
推导最优卡尔曼增益 K k
xˆ 最优化K：使后验估计 k 的协方差 Pk 达到最小。 x xˆ （换一个概念）也是使向量 k k 的二范数的
这里的权重系数也成为卡尔曼增益。
至此卡尔曼滤波计算原型公式基本结束， 下面要确定出最优系数K和在最优系数下 的后验估计误差的协方差矩阵的递推方 法
权重
残差
卡尔曼滤波
3 算法推导
推导后验协方差矩阵 Pˆ k 按照定义，我们从误差协方差 Pˆ k 开始推导如下：
P ˆkcoxvk(xˆk) 带入 x ˆkx ˆkK k(zkH x ˆk)
把上式带入3.5式，可以消去后面的两项，得：
P ˆkP ˆkK kH P ˆk
卡尔曼滤波
3 算法推导
整理，得：
P ˆk(IKkH)P ˆk
3.7
这个公式的计算比较简单，所以实际中总是使用这个 公式，但是需注意这公式仅在使用最优卡尔曼增益的 时候它才成立。如果算术精度总是很低而导致数值稳 定性出现问题，或者特意使用非最优卡尔曼增益，那 么就不能使用这个简化；必须使用3.5式表示的后验误 差协方差公式。
另外，状态转移矩阵A，输入矩阵B和测量方程2.2中 的矩阵H，在实际过程中都可能随时间变化而变化， 但在这儿假设为常数。
卡尔曼滤波
2 算法前提 先验估计和后验估计，及其误差
定义 xˆk Rn为根据上一次迭代计算结果而产生的
估计值，称为先验估计。
定义 xˆk Rn 为根据当前计算结果而产生的估计值，
数学期望值最小化的一个过程。 E{|xk xˆk|2}
这等同于后验估计的协方差矩阵的迹最小化
P{x |kx ˆk|2}trP ˆk
卡尔曼滤波
3 算法推导
首先展开3.4式，得：
P ˆkP ˆkKkHP ˆkP ˆkHTKkTKk (HP ˆkHTR)KkT 记： SkH P ˆkHTR
上面的式子可以写为：
内容提纲
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
3 算法推导
卡尔曼增益的物理意义
Kk
PˆkHT HPˆkHT R
其中：H矩阵为常量；Pˆ k 与过程激励噪声的协方差
矩阵Q有关；R为测量噪声的协方差矩阵。
取值范围： Kk[0,H1]
当 R 趋向于零时，有： limKk H1 R0
当 Pˆ k
趋向于零时，有： limKk 0 Pˆk0
卡尔曼滤波
3 算法推导
P ˆ k P ˆ k K k H P ˆ k P ˆ k H T K k T K k S k K k T
3.5
卡尔曼滤波
3 算法推导
把 tr (Pˆ k ) 对 Kk 求导，并令导数=0，则可以得到 tr (Pˆ k )
取最小值时的最优化 K k 的值。
dt(P ˆrk)(H P ˆk)TP ˆkHT(SkK kT)T(K kSk) dk K 2P ˆkHT2K kSk0
2 算法前提
随机离散系统模型
定义随机离散时间过程的状态向量 xk Rn，该过
程由以下离散随机差分方程描述：
x k A k 1 x B k 1 u w k 1 2.1
nx1
nxn
nxn nx1
nx1
假设系统满足可观性要求，定义观测向量 zk R，m
得到观测方程：
zkHk xvk
2.2
mx1 mxn
意义：决定了最优估计组成比例的“调节器”
当 R 趋向于零时，有： limKk H1 R0
测量噪声 V=0
mx1
此时3.3式改为
Байду номын сангаас
x ˆk x ˆk H 1 (zk H x ˆk) H 1 zk
系统表现为完全取测量值作为状态的后验估计值，而 系统的先验状态估计完全被抛弃。
反之当 Pˆ k 趋向于零时，根据式3.2可知，Q=0
P ˆk co x k v (x ˆk( K k (zk H x ˆk)))
再带入 zkHk xvk
P ˆk co x k ( v x ˆk ( K k (H k v x k H x ˆ k ))
卡尔曼滤波
3 算法推导
整理测量误差向量，得：
P ˆk co I K v k H ) (x k ( ( x ˆ k ) K k v k )
4.4
P ˆk(IKkH)P ˆk
4.5
卡尔曼滤波
4 算法总结
算法框图
1、状态估计向量的运算流程图
时间更新（预测） 测量更新（校正）
uk1 B
xˆ k
xˆk 1 A
H
zˆk
z k
K
xˆk
卡尔曼滤波
4 算法总结
算法框图
2、误差协方差的运算流程图
时间更新（预测）
测量更新（修正）
Pˆk 1
引入常用数学公式？？ dt(rBA)CBTCT dA
解得： K k P ˆ k H T S k 1 P ˆ k H T ( H P ˆ k H T R ) 1
3.6
卡尔曼滤波
3 算法推导
化简后验误差协方差公式
在卡尔曼增益等于上面导出的最优值时，计算后验协 方差的公式可以进行简化。
对于卡尔曼增益公式 KkP ˆkHTSk1 在卡尔曼增益公式两侧同时右乘 SkKkT 得： K kS kK kTP ˆkH TK kT
nx1
易知，此时系统完全抛弃测量值，取先验估计值
卡尔曼滤波
4 算法总结
算法描述
卡尔曼滤波器用反馈控制的方法估计过程状态。估计 过程某一时刻的状态，然后以（含噪声）测量值得方 式获得反馈，因此卡尔曼滤波器可以分为两个部分：
1、时间更新方程 负责向前推算当前状态向量和误差协方差估计的值， 为下一个时间状态构成先验估计。
卡尔曼滤波
1 发展概述
优势：
1、采用物理意义较为直观的时域状态空间 2、仅需要前后两步的数据，数据存储量较小 3、使用比较简单的递推算法，便于在计算机上实现 4、不仅适用于平稳过程，还可以推广到非平稳随机 过程的情况
发展：
卡尔曼滤波器已成为推广研究和应用的主题，尤其是 在自主或协助导航领域。
卡尔曼滤波
称为后验估计。
定义先验估计误差为 定义后验估计误差为
ekxkx ˆk
nx1 真值
ekxkx ˆk
nx1 真值
2.5
2.6
卡尔曼滤波
2 算法前提
定义先验估计误差的协方差为
PkE {ek,ekT}coek v )(
定义 后验估计误差的协方差为
PkE {ek,ekT}coek v )(
2.7
2.8
卡尔曼滤波
2、测量更新方程 负责将先验估计和新的测量变量结合已构成改进后的 后验估计。
卡尔曼滤波
4 算法总结
时间更新方程
x ˆk A x ˆk 1 B k u 1
4.1
P ˆkAP ˆk1ATQ
4.2
测量更新方程
K k P ˆk H T(H P ˆk H T R ) 1 4.3
x ˆkx ˆk K (zk H x ˆk)
3 算法推导
目的描述：
在系统结构已知的情况下，给定k时刻的状态观测向
z xˆ 量 k，求k时刻的系统状态向量的最优估计 k ，使
得 Pk最小。
基本思路：
xˆ xˆ 1、根据 k 1 计算出k时刻的先验估计 k
x ˆk A x ˆk 1 Bk u 1
3.1
同时也产生了先验估计的协方差矩阵 Pˆ k 的递推公式
因为噪声项与其他项不相关，协方差=0，所以有：
P ˆk co I v K k H ()x ( k ( x ˆk ) )co K k v k v )
利用协方差矩阵性质，提出常数矩阵，得：
P ˆk(IK kH)coxk v(x ˆk)I(K kH)T K kcovk v )K (kT
卡尔曼滤波
3 算法推导
也可以使用一个离线模拟滤波器与在线滤波器进行实 时对比的方式来实时优化滤波器的参数（Q和R）。
卡尔曼滤波
Q
A·AT