卡尔曼滤波器及其工程应用

第27卷第 1期 2 0 0 8年 3月 计 算 技 术 与 自 动 化 Computing Technology and Automation Vo1．27．No．1 Mar．2 0 0 8 文章编号：1003—6199{2008)01—0136—04 卡 尔曼 滤 波 器及 其 工程 应 用 张满生 一，张学庄 (1．湖南工业大学 电气与信息工程学院，湖南 株洲 412008；2．中南大学 信息物理学院，湖南 长沙 410083) 摘 要：卡尔曼于 1960年提 出了离散 系统线性滤波的递推求解方法即卡 尔曼滤波算法。该滤波算法 是基于线性最小平方法的、进行有效递推计算的一组数学方程式，算法功能强大，支持对过去、现在和将来 状态的估算。本文旨在对离散系统的卡尔曼滤波作应用方面的介绍：包括对基本离散卡尔曼滤波器的描述 与讨论 ；对扩展型卡尔曼滤波的讨论 ；应用实例进行分析和仿真；讨论卡尔曼滤波在汽车定位与导航工程上 的应用。 关键词：卡尔曼滤波；随机变量；测量；导航定位 中图分类号：TP13：0 231．1 文献标识码：A Kalman Filter and its Engineering Application ZHANG Man—sheng 一．ZHANG Xue—zhuang (1，Hunan university of technology，Zhuzhou 4 12000，China； 2，College of Information and Physics，Central south university，Changsha 410083，China) Abstract：Kalman published his famous paper in 1960．Kalman filter is a set of mathematical equations that provides an effi— cient computational(recursive)solution of the least—squares method．The filter is very powerful in several aspects：it supports es— timations of past，present，and even future states，and it can do so even When the precise nature of the modeled system is un． known．This paper is tO provide a practical introduction tO the discrete Kalman filter：including a description and some discussion of the basic discrete Kalman filter，a derivation，description and some discussion of the extended Kalman filter，an analysing and simu· lating a simple example．Fanally its some engineering applications were introduced． Key words：kalman filtering；stochastic variable；measurement；locating and navigation 1 离散卡尔曼滤波器 1960年，卡尔曼发表了关于描述离散系统线 性滤波问题的递推算法的著名论文-1]。自那时 起，由于数字计算技术的巨大进步，卡尔曼滤波方 法得到了深人的研究和广泛的工程应用，特别是在 自主或辅助定位导航领域，在测量和信号处理及自 动控制领域。国内较早详细介绍卡尔曼滤波理论 的书籍是敬喜-2 J1973年的《卡尔曼滤波器及其应 用基础》。此外，Gelb，Grewa，Lewis，Jacobs 等 也分别对卡尔曼滤波作出了进一步深入的研究，至 今，各类改进型卡尔曼滤波方法，如：内模自适应卡 尔曼滤波-4 J、带随机遗忘因子的自适应卡尔曼滤 波、与各类传感器信息融合相结合的卡尔曼滤波方 法[5][ ]都是控制理论的研究热点。 1．1 卡尔曼滤波对象的数学模型 卡尔曼滤波对线性离散时间控制过程的状态 的估算的一般问题，用线性随机差分方程表示： = Az 一1+Bu 一1+Wk一1 (1) m 维的观测方程为： 22 =Hxk+Vk (2) 其中用随机变量 叫 和 分另4表示过程噪声 和测量噪声。设两者之间是彼此独立，且符合 标准 excel标准偏差excel标准偏差函数exl标准差函数国标检验抽样标准表免费下载红头文件格式标准下载 正态分布的白噪声，即： 收稿 日期 ：20(17—07—31 作者简介：张满生(1962一)，男，湖南浏阳人，教授，研究方向：智能交通控制(E—mail：lymszhang@163，com)；张学庄(1940一)，男，江西 萍乡人，教授，博士生导师，研究方向：光电子测量仪器与系统。 维普资讯 http://www.cqvip.com 第 27卷第 1期 张满生等：卡尔曼滤波器及其工程应用 137 ⋯ 1．3 离散卡尔曼滤波算法 一 卡尔曼滤波利用反馈控制思想对过程进行估 定义第 k步的状态先验估计为 ：∈R ，即用 早于第 k步的过程辨识对第k步进行估计。而用 ∈R 表示在第k步得到测量值 后的状态后验估 计。并且，定义先验与后验估计之误差为： Pi= ．27女一 i和 e女= ．27女一 女 (4) 其中 ．27 是真实状态。先验估计误差协方差和 后验估计误差的协方差分别为 Pi=E Eei~ek丁] (5) Pk=E[eke ] (6) 在推导卡尔曼滤波方程时，首先寻求一个计算 后验估计 ^的： 程式。 2女=2i+K( 女一H2i) (7) 即以先验估计 i结合实际观测值 与测量 预报H2i之差的加权值，两者作线性组合，用于计 算后验估计 。式中的差值项( 一H2i)称为观 测新值，或观测残差。该残差反映了预报观测值与 实际观测值的偏差。若残差为零则说明两者完全一 致。加权值 K，称为卡尔曼滤波增益，使(6)最小化 的 K值结果如下： K ’(HP~H丁+R) = (8) 算。滤波器在某些时间点上估算过程状态，然后应 用观测量进行反馈。估算算法如图 1所示。 时间更新方程如下： 土i= 触 一^1+Bu^一1 (9) Pi=AP^ 一1A +Q (10) 注意在上两式中，都是用 k一1步估算 k步， 即作先验估计。 图 1 离散 卡 尔曼滤波迭代循 环 观测更新方程式如下： K女= PiH丁(HPiH丁+R)一 (11) 女= i+K女( 一H2i) (12) Pk= ( —KH^)Pi (13) 观测更新的第一步是计算卡尔曼增益 K ，然 后按实际测量以获得观测值 ，再按(12)计算后 验估计 ，最后用(13)计算后验误差协方差。图2则 更清晰地表明了卡尔曼滤波的全貌。 代入 一。和 一。的初始条件 图2 卡尔曼滤运运算的全过程 2 扩展型卡尔曼滤波器(EKF)[6] 2．1 估计过程 当估计过程与该过程的观测值是非线性关系 时，对当前的均值和协方差作线性化处理后的卡尔 曼滤波器称为扩展型卡尔曼滤波器 (EKF)。类似 于泰勒级数，围绕当前的估计点，应用对状态方程 和测量方程求偏微分，便实现了非线性关系的估计 过程的线性化处理。 设过程的非线性状态随机差分方程为： ．27 = f(．27 一1，M 一1，叫 一1) (1) 测量方程为 (量测量 )： =h(．27 ， ) (2) 维普资讯 http://www.cqvip.com 计算技术与自动化 2008年 3月 当然，实际上不可能了解到每一采样时刻的 ^和 值，在将它们视为零值时，可近似地将状 态向量和观测向量表示为： =f( 一1，U^一1，0) (3) = h( ，0) (4) 其中 一1是上一步 k一1的状态后验估计。须指出 的是 EKF只是基于贝叶斯(Bayes)准则作线性化 后，所得到近似最优的一个特殊状态估计器。 2．2 EKF的计算起点(源点) 对方程(3)、(4)作线性化，并重写为以下控制 方程： z点≈ 七+A(z七一1—2七一1)+Ww七一1 (5) z点≈ 乏 +H(z七一 点)+Vv点 (6) 其中：z^ 和 是实际的状态向量和测量向量江 和^ 是源自(3)和(4)近似的状态向量及测量向量； 2^ 是第k步的状态后验估计；随机变量 和^ ^ 是 过程噪声和测量噪声。A是非线性函数厂(⋯)关于 z的偏导数的雅可比(Jacobian)矩阵，即： 川 = 考 (ff2k_1,Uk_1,0) (7) 而 w，H，w，V均为相应的雅可比矩阵，同理 计算如下： w (ff2k_1,Uk_1,0)； HI ]={ ( 0)； V )； 完整的 EKF方程如下(推导略)，为了与前述 方法一致，仍然用2i取代 ，并且用下标k标注在 雅可比阵 A，w ，H，V，以表明其随时间而变。 2i=f(2 一1，U^一1，0) (8) Pi= A 一1A +w Q 一^1 V (9) 该两式构成 EKF的时问更新方程。而EKF的 测量更新如下： K^= p~HT(HkP；HT+ R^ ) (10) 2^ = 2i+K (^ ^一h(2i，0)) (11) = (J—K )Pi (12) 3 卡尔曼滤波实例 前面分别介绍了离散卡尔曼滤波器的基本型 及扩展型，下面介绍卡尔曼滤波在车辆定位导航系 统中的应用[7][8l，以帮助理解。 (1)车辆定位与导航系统的数学模型 豳 匡 ；]+I l + ]∞c c 式中为观测矩阵，且H(足)=[ ；：]； 维普资讯 http://www.cqvip.com 第27卷第 1期 张满生等：卡尔曼滤波器及其工程应用 139 Sage自适应卡尔曼滤波，对提高定位精度均有较大 的作用。从标准滤波的35．35米提高到改进型自适 标准卡尔曼滤波 采样时间 (×5s) 图 3 标；隹卡尔曼滤波法滤波效果 参考文献 [1] Kalrnan，R．E．1960．“A New Approach to Linear Filtering and Prediction Pml：}lems”[J]．Transaction of the ASME—Jour． nnl of Basic Engino．~ring，PP．35—45(March 1960)． [2] 敬 喜．卡尔曼滤波：器及其应用基础[M]．北京：国防工业出版 社 ，1973． [3] Lewis，Richard．1986．Optimal Estimation with an lntroduc- tion Stochastic Control Theory．John Wiley＆ Sons．1nc． [4] 徐寿宁、张建华．内模自适应卡尔曼滤波新方法及其在 GPS 应滤波的20．23米。 自适应卡 尔曼滤波 采样时间 (×5s) 图 4 改进型 自适应卡 尔曼滤波效果 信号估计中的应用[J]．北京工业大学学报，2001，27(2)：148 — 156． 秦永元，张洪钺，汪叔华．卡尔曼滤波与组合导航原理[M]． 西安：西北工业大学出版社，1998． 张满生，张学庄．ABS传感器模型及多传感器信息融合在车 辆定位中的应用[J]．传感技术学报，2006，4，(19)，415— 418． Greg W elch an d Gary Bishop ，An Introduction to the Kalman Filter，Department of Co mputer ScienceUniversity of Noah Carolina at Chapel Hil1．2003，5． 张满生，张学庄，等．新型GPS动态定位自适应卡尔曼滤波方 法[J]．中南工业大学学报，2003，5(34)，543—546． (Ⅲv (Ⅲv 维普资讯 http://www.cqvip.com