1、 工业和信息产业职业教育教学指导委员会“十二五”规划教材 电子工程技术丛书 卡尔曼滤波原理及应用 MATLAB 仿真 黄小平 王 岩 编著 内 容 简 介 本书主要介绍数字信号处理中的卡尔曼（Kalman）滤波算法及在相关领域应用的相关内容。全书共 7 章。第 1 章为绪论。第 2 章介绍 MATLAB 算法仿真的编程基础。第 3 章介绍线性 Kalman 滤波。第 4 章讨论扩展Kalman 滤波，并介绍其在目标跟踪和制导领域的应用和算法仿真。第 5 章介绍 UKF 滤波算法，同时也给出其应用领域内的算法仿真实例。第6 章介绍了交互多模型Kalman 滤波算法。第 7 章介绍Simulink
2、 环境下，如何通过模块库和 S 函数构建Kalman 滤波器，并给出了系统是线性和非线性两种情况的滤波器设计方法。本书可以作为电子信息类各专业高年级本科生和硕士、博士研究生数字信号处理课程或者 Kalman 滤波原理的教材，也可以作为从事雷达、语音、图像等传感器数字信号处理的教师和科研人员的参考书。未经许可，不得以任何方式复制或抄袭本书之部分或全部内容。版权所有，侵权必究。图书在版编目（CIP）数据 卡尔曼滤波原理及应用：MATLAB 仿真/黄小平，王岩编著.北京：电子工业出版社，2015.7 ISBN 978-7-121-26310-1.卡 .黄 王 .卡尔曼滤波系统仿真Matlab 软件
3、.O211.64-39 中国版本图书馆 CIP 数据核字（2015）第 127904 号 责任编辑：刘海艳（）印 刷：装 订：出版发行：电子工业出版社 北京市海淀区万寿路 173 信箱 邮编：100036 开 本：7871 092 1/16 印张：11.75 字数：264 千字 版 次：2015 年 7 月第 1 版 印 次：2015 年 7 月第 1 次印刷 印 数：3 000 册 定价：39.80 元 凡所购买电子工业出版社图书有缺损问题，请向购买书店调换。若书店售缺，请与本社发行部联系，联系及邮购电话：（010）88254888。质量投诉请发邮件至 ，盗版侵权举报请发邮件至 。服务热线：
4、（010）88258888。前 言 随着科技的发展，在雷达、声呐、通信、视频图像处理、故障诊断等领域，对信号检测和状态参数估计的研究，有着重要的价值。在所有数字信号处理应用系统中，传感器数据采集是重要的一环。所有传感器测量的数据都受到噪声的污染，噪声不能消除，只能尽最大限度地降低。例如，在目标跟踪中，传感器一般是测量观测站与目标之间的距离、角度等信息，这些信息往往会受到高斯、非高斯噪声的污染，导致观测站不能准确地估计目标的状态，那么这时对测量数据进行滤波就显得很有必要了。卡尔曼（Kalman）滤波是噪声处理的利器，目前关于卡尔曼滤波的论文非常多，专著也不少，但是在阐述 Kalman 滤波原理时
5、，大多文献只停留在公式推导和文字介绍上，而且各作者公式表示习惯不一样导致要理解 Kalman 滤波原理非常困难，在编程仿真上也存在诸多疑问，因此很多读者在刚开始接触该滤波算法时总是疑虑重重。鉴于此，本书在介绍 Kalman 滤波原理时，加入了大量的应用仿真实例。本书写作时尽量避免繁缛的公式推导，用通俗易懂的语言文字，配用详细的MATLAB 仿真程序和中文注释，读者可以对照核心公式和程序注释理解卡尔曼滤波原理。本书的主要内容是卡尔曼滤波的状态估计方法：应用在线性领域中，主要是经典卡尔曼滤波；而应用在非线性系统中，主要是扩展卡尔曼滤波和无迹 Kalman 滤波。当然在很多文献中有各种卡尔曼滤波的衍
6、生算法，例如信息卡尔曼、强跟踪卡尔曼、集合卡尔曼、容积卡尔曼和神经网络卡尔曼等。笔者认为，其他衍生算法都是以经典卡尔曼为母体的，只要掌握经典 Kalman 滤波算法的核心和精髓即能触类旁通，学一知百。同样地，在研究各种衍生算法之前，必须先掌握经典算法。在应用实例方面，读者一定要掌握系统建模问题。所谓系统建模，是指 Kalman滤波中的状态方程和观测方程的建立。这两个方程中的状态、矩阵参数的设置不同，就代表着不同的系统。经典卡尔曼滤波和交互多模型卡尔曼滤波属于线性滤波器，这些算法的应用领域主要有温度测量、GPS 导航、石油地震勘探、视频图像中的目标检测和跟踪。非线性滤波器主要有 EKF 和 UK
7、F 算法，应用实例主要是 IV 卡尔曼滤波原理及应用MATLAB 仿真 纯方位、纯距离的目标跟踪、寻的制导系统。在工程应用中，系统模型是千奇百怪的，本书不可能列举所有的应用。鉴于此本书给出了通用的一维、二维和四维状态系统滤波问题，读者掌握这些通用模型仿真，在遇到其他信号处理模型时即可得心应手了。参加本书编写的还有王岩、聂金平、闫芬菲、陈冰洁、田龙飞、李超、王夏静、钱琛、杨刚、李超（小）、许蓓蓓。本书的编辑和勘误，得到了北航同课题组实验室的学长的帮助，感谢王驭风、刘涛、徐建伟的指导。另外特别感谢北京理工大学何绍敏的全力相助，感谢一直支持和帮我修改错误的各位网友！希望本书对于从事相关领域的研究者有
8、所帮助。由于作者的水平有限，其中难免有疏漏和不足之处，恳请读者提出宝贵的意见，我的邮箱xiaoping_。2015 年 4 月 目 录 第 1 章 绪论 1 1.1 滤波的基础知识 1 1.2 Kalman 滤波的背景 1 1.3 Kalman 滤波的发展过程 2 1.4 Kalman 滤波的应用领域 4 第 2 章 MATLAB 仿真基础 6 2.1 MATLAB 简介 6 2.1.1 MATLAB 发展历史 6 2.1.2 MATLAB 7.1 的系统简介 7 2.1.3 M 文件编辑器的使用 10 2.2 数据类型和数组 12 2.2.1 数据类型概述 12 2.2.2 数组的创建 13
9、 2.2.3 数组的属性 15 2.2.4 数组的操作 16 2.2.5 结构体和元胞数组 19 2.3 程序设计 21 2.3.1 条件语句 21 2.3.2 循环语句 23 2.3.3 函数 25 2.3.4 画图 27 2.4 小结 29 第 3 章 线性 Kalman 滤波 30 3.1 Kalman 滤波原理 30 3.1.1 射影定理 30 3.1.2 Kalman 滤波器 33 VI 卡尔曼滤波原理及应用MATLAB 仿真 3.1.3 Kalman 滤波的参数处理 37 3.2 Kalman 滤波在温度测量中的应用 39 3.2.1 原理介绍 39 3.2.2 MATLAB 仿真
10、程序 42 3.3 Kalman 滤波在自由落体运动目标跟踪中的应用 44 3.3.1 状态方程的建立 44 3.3.2 MATLAB 仿真程序 47 3.4 Kalman 滤波在船舶 GPS 导航定位系统中的应用 50 3.4.1 原理介绍 50 3.4.2 MATLAB 仿真程序 53 3.5 Kalman 滤波在石油地震勘探中的应用 55 3.5.1 石油地震勘探白噪声反卷积滤波原理 55 3.5.2 石油地震勘探白噪声反卷积滤波仿真实现 57 3.5.3 MATLAB 仿真程序 58 3.6 Kalman 滤波在视频图像目标跟踪中的应用 60 3.6.1 视频图像处理的基本方法 61
11、3.6.2 Kalman 滤波对自由下落的皮球跟踪应用 68 3.6.3 目标检测 MATLAB 程序 70 3.6.4 Kalman 滤波视频跟踪 MATLAB 程序 72 第 4 章 扩展 Kalman 滤波 77 4.1 扩展 Kalman 滤波原理 77 4.1.1 局部线性化 77 4.1.2 线性 Kalman 滤波 79 4.2 简单非线性系统的扩展 Kalman 滤波器设计 80 4.2.1 原理介绍 80 4.2.2 标量非线性系统 EKF 的 MATLAB 程序 83 4.3 EKF 在目标跟踪中的应用 84 4.3.1 目标跟踪数学建模 84 4.3.2 基于观测距离的
12、EKF 目标跟踪算法 85 4.3.3 基于距离的目标跟踪算法 MATLAB 程序 87 VII 目录4.3.4 基于 EKF 的纯方位目标跟踪算法 89 4.3.5 纯方位目标跟踪算法 MATLAB 程序 91 4.4 EKF 在纯方位寻的导弹制导中的应用 94 4.4.1 三维寻的制导系统 94 4.4.2 EKF 在寻的制导问题中的算法分析 96 4.4.3 仿真结果 97 4.4.4 寻的制导 MATLAB 程序 99 第 5 章 无迹 Kalman 滤波 103 5.1 无迹 Kalman 滤波原理 103 5.1.1 无迹变换 103 5.1.2 无迹 Kalman 滤波算法实现
13、105 5.2 无迹 Kalman 滤波在单观测站目标跟踪中的应用 107 5.2.1 原理介绍 107 5.2.2 仿真程序 108 5.3 UKF 在匀加速度直线运动目标跟踪中的应用 111 5.3.1 原理介绍 111 5.3.2 仿真程序 113 5.4 UKF 与 EKF 算法的应用比较 116 第 6 章 交互多模型 Kalman 滤波 119 6.1 交互多模型 Kalman 滤波原理 119 6.2 交互多模型 Kalman 滤波在目标跟踪中的应用 122 6.2.1 问题描述 122 6.2.2 IMM 滤波器设计 123 6.2.3 仿真分析 124 6.2.4 IMM K
14、alman 滤波算法 MATLAB 仿真程序 126 第 7 章 Kalman 滤波的 Simulink 仿真 132 7.1 Simulink 概述 132 7.1.1 Simulink 启动 132 7.1.2 Simulink 仿真设置 134 7.1.3 Simulink 模块库简介 139 VIII卡尔曼滤波原理及应用MATLAB 仿真 7.2 S 函数 143 7.2.1 S 函数原理 143 7.2.2 S 函数的控制流程 147 7.3 线性 Kalman 的 Simulink 仿真 148 7.3.1 一维数据的 Kalman 滤波处理 148 7.3.2 状态方程和观测方程
15、的 Simulink 建模 154 7.3.3 基于 S 函数的 Kalman 滤波器设计 160 7.4 非线性 Kalman 滤波 167 7.4.1 基于 Simulink 的 EKF 滤波器设计 167 7.4.2 基于 Simulink 的 UKF 滤波器设计 174 7.5 小结 179 第1章 绪 论 1.1 滤波的基础知识 什么是滤波？滤波一词起源于通信理论，它是从含有干扰的接收信号中提取有用信号的一种技术。而更广泛地，滤波是指利用一定的手段抑制无用信号，增强有用信号的数字信号处理过程。无用信号，也叫噪声，是指观测数据对系统没有贡献或者起干扰作用的信号。在通信中，无用信号表现为
16、特定波段频率、杂波；在传感器数据测量中，无用信号表现为幅度干扰。例如，在温度测量中，传感器测量值与真实温度之间往往有一定的随机波动，这个波动就是随机干扰。其实噪声是一个随机过程，而随机过程有其功率谱密度函数，功率谱密度函数的形状决定了噪声的“颜色”。如果这些干扰信号幅度分布服从高斯分布，而它的功率谱密度又是均匀分布的，则称它为高斯白噪声。高斯白噪声是大多数传感器所具有的一种测量噪声。在工程应用中，如雷达测距、声呐测距、图像采集、声音录制等，只要是传感器采集和测量的数据，都携带噪声干扰。这种影响有的很微小，有的则会使信号变形、失真，有的严重导致数据不可用。那么滤波也不是万能的，滤波只能最大限度降低噪声的干扰，即有的滤波是不能完全消除噪声，有的则可能完全消除。1.2 Kalman 滤波的背景 滤波理论就是在对系统可观测信号进行测量的基础上，根据一定的滤波准则，采用某种统计量最优方法，对系统的状态进行估计的理论和方法。所谓最优滤波或最优估计是指在最小方差意义下的最优滤波或估计，即要求信号或状态的最优估值应与相应的真实值的误差的方差最小。经典最优滤波理论包括 Wiener（维纳）滤波理论和 K