卡尔曼滤波器原理

1、卡尔曼滤波器原理卡尔曼滤波器原理 林斌林斌 P201109026 2 1 2 3 4 内容提纲内容提纲 发展概述 算法前提 算法推导 算法总结 卡尔曼滤波 5 算法扩展 3 Rudolf (Rudy) Emil Klmn （1930 ） 卡尔曼滤波 工程背景：1960s 航空航天工程突飞猛进 电子计算机又方兴未艾 正式提出： 一种关于线性过滤和预测难题的新方法1960 A new approach to linear filtering and prediction problems ， 1960 1 发展概述 4 卡尔曼滤波 1 发展概述 优势： 1、采用物理意义较为直观的时域状态空间 2、

2、仅需要前后两步的数据，数据存储量较小 3、使用比较简单的递推算法，便于在计算机上实现 4、不仅适用于平稳过程，还可以推广到非平稳随机 过程的情况 发展： 卡尔曼滤波器已成为推广研究和应用的主题，尤其是 在自主或协助导航领域。 5 卡尔曼滤波 2 算法前提 随机离散系统模型 定义随机离散时间过程的状态向量 ，该过 程由以下离散随机差分方程描述： 111kkkkwBuAxx kkkvHxz m kRz n kRx 1 . 2 假设系统满足可观性要求，定义观测向量 ， 得到观测方程： 2 . 2 随机信号 和 分别表示过程激励噪声和观测噪 声，并假设它们是相互独立并满足正态分布的白噪声。 kvkw

3、nx1 nxn nxn nx1 nx1 mx1 mxn mx1 6 卡尔曼滤波 2 算法前提 即有： 过程激励噪声 观测噪声 实际过程中过程激励噪声协方差矩阵Q和观测噪声协 方差矩阵R可能随着每次迭代计算而变化，但在这里 我们假设它们为常数。 另外，状态转移矩阵A，输入矩阵B和测量方程2.2中 的矩阵H，在实际过程中都可能随时间变化而变化， 但在这儿假设为常数。 3 . 2 4 . 2), 0(RNvk ), 0(QNwk 7 卡尔曼滤波 2 算法前提 先验估计和后验估计，及其误差 定义 为根据上一次迭代计算结果而产生的 估计值，称为先验估计。 定义 为根据当前计算结果而产生的估计值， 称为后

4、验估计。 定义先验估计误差为 定义后验估计误差为 n kRx n kRx kkkxxe nx1 真值 5 . 2 kkkxxe nx1 真值 6 . 2 8 卡尔曼滤波 2 算法前提 定义先验估计误差的协方差为 定义 后验估计误差的协方差为 )cov(,kkkkeeeEP T )cov(,EkkkkeeeP T 7 . 2 8 . 2 9 卡尔曼滤波 目的描述： 在系统结构已知的情况下，给定k时刻的状态观测向 量 ，求k时刻的系统状态向量的最优估计 ，使 得 最小。 基本思路： 1、根据 计算出k时刻的先验估计 同时也产生了先验估计的协方差矩阵 的递推公式 3 算法推导 kz k x kP 1

5、kxk x 11kkkBuxAx 1 . 3 kP QAPAP T kk1 2 . 3 10 卡尔曼滤波 2、根据先验估计 计算出k时刻的观测向量的估计 3、计算实测值 与估计 的差，以此来修正之 前的先验估计 ，得到后验估计 把 带入： 3 算法推导 )(kkkkkxHzKxx3 . 3 k x kkxHz kzk z k x k x 权重 残差 这里的权重系数也成为卡尔曼增益。 至此卡尔曼滤波计算原型公式基本结束， 下面要确定出最优系数K和在最优系数下 的后验估计误差的协方差矩阵的递推方 法 )(kkkkkzzKxx k z 11 卡尔曼滤波 推导后验协方差矩阵 按照定义，我们从误差协方差

6、 开始推导如下： 带入 再带入 3 算法推导 kP )cov( kkkxxP kP )(cov( kkkkkkxHzKxxP )(kkkkkxHzKxx kkkvHxz )(cov( kkkkkkkxHvHxKxxP 12 卡尔曼滤波 整理测量误差向量，得： 因为噪声项与其他项不相关，协方差=0，所以有： 利用协方差矩阵性质，提出常数矩阵，得： 3 算法推导 )(cov( kkkkkkvKxxHKIP )cov()(cov( kkkkkkvKxxHKIP T kkk T kkkkk KvK HKIxxHKIP )cov( )(cov()( 13 卡尔曼滤波 如果记 ，则有： 3 算法推导 kk

7、kPxx )cov( T kk T kkkkRKKHKIPHKIP)( )( 4 . 3 14 卡尔曼滤波 推导最优卡尔曼增益 最优化K：使后验估计 的协方差 达到最小。 （换一个概念）也是使向量的二范数的 数学期望值最小化的一个过程。 这等同于后验估计的协方差矩阵的迹最小化 3 算法推导 kK k x kP kkxx | 2 kkxxE kkkPtrxxP | 2 15 卡尔曼滤波 首先展开3.4式，得： 记： 上面的式子可以写为： 3 算法推导 T k T k k T k T kkkkk KRHPH KKHPPHKPP ) ( RHPHS T kk T kkk T k T kkkkkKSK

8、KHPPHKPP 5 . 3 16 卡尔曼滤波 把 对 求导，并令导数=0，则可以得到 取最小值时的最优化 的值。 解得： 3 算法推导 ) (kPtr TT CB dA BACdtr )( 引入常用数学公式？ kK) (kPtr kK 02 2 )()( ) ( ) ( kk T k kk TT kk T k T k k k SKHP SKKSHPPH dK Pdtr 11 ) ( RHPHHPSHPK T k T kk T kk 6 . 3 17 卡尔曼滤波 化简后验误差协方差公式 在卡尔曼增益等于上面导出的最优值时，计算后验协 方差的公式可以进行简化。 对于卡尔曼增益公式 在卡尔曼增益公

9、式两侧同时右乘 得： 把上式带入3.5式，可以消去后面的两项，得： 3 算法推导 1 k T kkSHPK T kkKS T k T k T kkkKHPKSK kkkkPHKPP 18 3 算法推导 整理，得： 这个公式的计算比较简单，所以实际中总是使用这个 公式，但是需注意这公式仅在使用最优卡尔曼增益的 时候它才成立。如果算术精度总是很低而导致数值稳 定性出现问题，或者特意使用非最优卡尔曼增益，那 么就不能使用这个简化；必须使用3.5式表示的后验误 差协方差公式。 kkkPHKIP )( 7 . 3 19 卡尔曼滤波 卡尔曼增益的物理意义 其中：H矩阵为常量； 与过程激励噪声的协方差 矩阵

10、Q有关；R为测量噪声的协方差矩阵。 取值范围： 当 R 趋向于零时，有： 当 趋向于零时，有： 3 算法推导 0lim 0 k P K k kP RHPH HP K T k T k k kP 1 0 lim HKk R , 0 1 HKk 20 卡尔曼滤波 意义：决定了最优估计组成比例的“调节器” 当 R 趋向于零时，有： 测量噪声 V=0 此时3.3式改为 系统表现为完全取测量值作为状态的后验估计值，而 系统的先验状态估计完全被抛弃。 反之当 趋向于零时，根据式3.2可知，Q=0 易知，此时系统完全抛弃测量值，取先验估计值 3 算法推导 kP 1 0 lim HKk R mx1 kkkkkz

11、HxHzHxx 11 )( nx1 21 卡尔曼滤波 算法描述 卡尔曼滤波器用反馈控制的方法估计过程状态。估计 过程某一时刻的状态，然后以（含噪声）测量值得方 式获得反馈，因此卡尔曼滤波器可以分为两个部分： 1、时间更新方程 负责向前推算当前状态向量和误差协方差估计的值， 为下一个时间状态构成先验估计。 2、测量更新方程 负责将先验估计和新的测量变量结合已构成改进后的 后验估计。 4 算法总结 22 卡尔曼滤波 时间更新方程 测量更新方程 4 算法总结 11kkkBuxAx1 . 4 2 . 4 1 ) ( RHPHHPK T k T kk 3 . 4 4 . 4)(kkkkxHzKxx QA

12、PAP T kk 1 5 . 4kkkPHKIP )( 23 卡尔曼滤波 4 算法总结 B A HK k x k x kz 1kx 1ku k z 时间更新（预测）测量更新（校正） 算法框图 1、状态估计向量的运算流程图 24 卡尔曼滤波 4 算法总结 算法框图 2、误差协方差的运算流程图 AATI -KkH kP Q 1 kP 时间更新（预测）测量更新（修正） kP 25 卡尔曼滤波 滤波器参数调整 在卡尔曼滤波器实际实现时，测量噪声R一般可以观 测得到，是滤波器的已知条件。观测测量噪声协方差 R可以通过离线试验获取。 而过程激励噪声协方差Q值比较难以确定，因为我们 无法直接观测到过程状态 。有时候可以通过Q的 手动选择，人为的为过程“注入”一个足够的不