《Kalman滤波理论及其在导航系统中的应用(第三版)》紧密结合Kalman滤波理论在导航、制导与控制领域的应用，系统介绍了Kalman滤波基础理论及其*新发展。主要内容包括Kalman滤波理论基础、随机线性系统Kalman滤波基本方程、Kalman滤波的稳定性及误差分析、实用Kalman滤波技术、鲁棒自适应滤波技术、联邦Kalman滤波、基于小波变换的多尺度Kalman滤波、扩展Kalman滤波、无迹Kalman滤波及粒子滤波等。《Kalman滤波理论及其在导航系统中的应用(第三版)》注重理论与工程实际相结合，在介绍理论基础上，还融入了作者及其他研究者的实际应用成果，理论与实践并重。

目录
第1章 绪论1
1.1 Kalman滤波理论基础1
1.1.1 滤波与估计2
1.1.2 线性*小方差估计4
1.1.3 正交投影定理6
1.1.4 白噪声与有色噪声7
1.2 Kalman滤波理论的发展及其应用8
思考题12
第2章 随机线性系统Kalman滤波基本方程13
2.1 随机线性系统的数学模型13
2.1.1 随机线性离散系统的数学模型13
2.1.2 随机线性连续系统的数学模型14
2.1.3 随机线性连续系统的离散化16
2.2 随机线性离散系统Kalman滤波方程19
2.2.1 随机线性离散系统Kalman滤波基本方程19
2.2.2 随机线性离散系统Kalman滤波方程的直观推导21
2.2.3 随机线性离散系统Kalman滤波方程的投影法推导23
2.3 随机线性连续系统Kalman滤波基本方程28
2.4 随机线性离散系统的*优预测与平滑31
2.4.1 随机线性离散系统的*优预测31
2.4.2 随机线性离散系统的*优平滑33
2.4.3 *优平滑方法在惯导系统中的应用34
2.5 Kalman滤波在车载惯导系统中的应用实例38
2.5.1 里程计误差建模38
2.5.2 紧耦合辅助模式下系统建模40
2.5.3 跑车实验验证及结果分析42
思考题44
第3章 Kalman滤波的稳定性及误差分析46
3.1 稳定性的概念46
3.2 随机线性系统的可控性与可观测性47
3.2.1 随机线性系统的可控性47
3.2.2 随机线性系统的可观测性48
3.3 Kalman滤波稳定性的判别49
3.3.1 随机线性系统的滤波稳定性判别49
3.3.2 特定条件系统的滤波稳定性判别51
3.4 Kalman滤波发散及误差分析54
3.4.1 Kalman滤波中的发散现象54
3.4.2 Kalman滤波的误差分析56
3.5 惯导系统可观测性与可观测度分析方法60
3.5.1 捷联惯导系统误差模型61
3.5.2 可观测性分析方法63
3.5.3 可观测度分析方法67
思考题76
第4章 实用Kalman滤波技术77
4.1 噪声非标准假设条件下的Kalman滤波77
4.1.1 存在确定性控制时的Kalman滤波77
4.1.2 白噪声相关条件下的Kalman滤波78
4.1.3 有色噪声条件下的Kalman滤波80
4.1.4 厚尾噪声条件下的Kalman滤波83
4.2 分解滤波87
4.2.1 非负定阵的三角形分解87
4.2.2 观测值为标量时误差方差平方根滤波89
4.2.3 序列平方根滤波92
4.2.4 UD分解滤波95
4.2.5 分解滤波在近地卫星GNSS自主定轨中的应用99
4.3 信息滤波105
4.3.1 信息平方根滤波105
4.3.2 厚尾噪声条件下的信息滤波109
思考题111
第5章 鲁棒自适应滤波技术113
5.1 系统的不确定性113
5.2 鲁棒控制技术基础114
5.2.1 基础知识114
5.2.2 H∞控制的标准设计问题115
5.2.3 Hamilton矩阵与H∞标准设计问题的求解116
5.3 H∞滤波119
5.3.1 H∞滤波问题的表达119
5.3.2 次优H∞滤波问题的解120
5.3.3 H∞滤波器的参数化121
5.3.4 GPS/INS组合导航系统H∞滤波122
5.4 强跟踪滤波125
5.4.1 强跟踪滤波器的引入125
5.4.2 基于强跟踪滤波的多传感器状态融合估计129
5.4.3 里程计辅助惯导系统动基座对准应用实例130
5.5 自适应滤波134
5.5.1 相关法自适应滤波135
5.5.2 Sage-Husa自适应Kalman滤波138
5.5.3 激光陀螺随机漂移自适应Kalman滤波145
思考题148
第6章 联邦Kalman滤波149
6.1 各子滤波器估计不相关条件下的联邦Kalman滤波算法149
6.2 各子滤波器估计相关条件下的联邦Kalman滤波算法151
6.2.1 信息分配原则与全局*优估计151
6.2.2 联邦滤波算法的时间更新154
6.2.3 联邦滤波算法的观测更新155
6.2.4 联邦滤波器的结构157
6.3 基于联邦Kalman滤波的惯导系统姿态组合算法159
6.3.1 基于姿态-位置-速度组合方式的联邦滤波器实现结构159
6.3.2 姿态组合观测方程160
6.3.3 仿真实验与结果分析161
6.4 基于联邦Kalman滤波的自主无人系统多源融合导航算法164
6.4.1 自主无人系统多源融合导航联邦Kalman滤波器结构165
6.4.2 自适应信息分配算法166
6.4.3 自适应联邦Kalman滤波算法跑车实验验证168
思考题170
第7章 基于小波变换的多尺度Kalman滤波171
7.1 小波变换基础171
7.1.1 小波变换概述171
7.1.2 多尺度分析173
7.1.3 Mallat算法177
7.2 多尺度系统理论179
7.3 动态系统的多尺度Kalman滤波181
7.4 多尺度Kalman滤波在导航系统中的应用184
7.4.1 系统描述184
7.4.2 多尺度模型的建立185
7.4.3 状态的多尺度估计187
7.4.4 组合导航系统的多尺度Kalman滤波仿真实验190
思考题193
第8章 扩展Kalman滤波195
8.1 普通扩展Kalman滤波195
8.1.1 随机非线性离散系统标称状态线性化滤波196
8.1.2 随机非线性离散系统扩展Kalman滤波198
8.1.3 扩展Kalman滤波在车辆GPS/DR组合定位系统中的应用200
8.2 多模型扩展Kalman滤波206
8.2.1 问题描述206
8.2.2 概率模型集设计206
8.2.3 多模型扩展Kalman滤波210
8.2.4 观测信息随机不可靠下的鲁棒多模型扩展Kalman滤波211
8.2.5 实验与结果分析212
思考题214
第9章 无迹Kalman滤波215
9.1 无迹变换与无迹Kalman滤波215
9.2 中心微分Kalman滤波218
9.3 平方根无迹Kalman滤波220
9.4 鲁棒无迹Kalman滤波222
9.4.1 鲁棒滤波算法代价函数的推导222
9.4.2 RDUKF算法流程224
9.4.3 RDUKF算法估计误差的随机有界性分析225
9.4.4 实验与结果分析229
9.5 自适应鲁棒无迹Kalman滤波233
9.5.1 代价函数构建233
9.5.2 基于自适应因子和*大相关熵准则的UKF算法设计236
9.5.3 实验与结果分析239
9.6 基于自适应鲁棒无迹Kalman滤波的重力匹配方法241
9.6.1 重力匹配算法误差模型建立241
9.6.2 基于ARUKF的重力匹配算法243
9.6.3 算法仿真分析248
9.7 无迹Kalman滤波在GNSS/INS松组合导航系统中的应用251
9.7.1 系统的状态方程251
9.7.2 系统的观测方程254
9.7.3 基于无迹Kalman滤波的传感器时延补偿255
9.7.4 实验与结果分析255
思考题260
第10章 粒子滤波261
10.1 **粒子滤波261
10.1.1 隐马氏模型与Bayes推断261
10.1.2 重要性采样263
10.1.3 序列重要性采样264
10.1.4 重采样法266
10.1.5 优选重要性密度函数法269
10.2 基于皮尔森系数驱动的粒子滤波270
10.2.1 皮尔森系数270
10.2.2 基于皮尔森系数驱动的粒子滤波设计271
10.2.3 仿真与对比分析272
10.3 无迹粒子滤波276
10.3.1 无迹粒子滤波算法276
10.3.2 无迹粒子滤波在捷联惯导初始对准中的应用279
思考题284
参考文献285

第1章 绪论
　　随着科学技术的飞速发展，网络、计算、数据、知识共同驱动着社会经济各领域持续创新发展。尤其是随着新一代信息技术的蓬勃崛起，我国相继提出了“互联网+”和“智能+”行动计划，使得信号与信息处理、计算机和自动控制等领域的重要性日益突出。陆海空天及工业过程的实际环境复杂多变，信息在传播过程中不可避免地混杂随机噪声或其他干扰信号。因此，对信号噪声进行处理的滤波理论及其技术的研究成为现代信息学科领域一个重要的研究方向。
　　滤波也称为状态估计。滤波就是从混杂在一起的信号中提取有用信息的技术，它是控制理论、计算机技术和概率论与数理统计的交叉产物。以现代控制理论为基础，基于线性系统模型和白噪声假设、基于*小均方误差准则的滤波算法及其优化算法在目标跟踪、信息融合、图像处理，以及导航与制导等诸多领域获得广泛应用。然而，在实际应用中，需要通过通信网络进行信息的接收和发送。在这个过程中，网络可能会受到系统本身和外界环境中的诸多不确定性因素的影响，如非高斯噪声(混合高斯噪声、散射噪声等)、非完整信息(异常值、数据缺失)等。基于*小均方误差准则的滤波算法及其优化算法对上述不确定因素的影响比较敏感，使得滤波算法在稳定性、鲁棒性等方面遇到挑战，导致滤波算法性能降低，甚至发散。因此，开展针对实际工程中含有诸多不确定因素的滤波问题研究，具有重要的理论意义和实际应用价值。
　　在现实世界中，不存在真正意义上的线性系统。实际系统都是非线性系统，而且大多数系统具有强非线性特性。因此这类系统很难利用线性微分方程或差分方程来描述，如惯导系统、卫星姿态控制等。非高斯噪声、非完整信息等复杂情况下的非线性系统的状态估计问题也是当前研究的热点和难点。
　　本书以导航系统的应用需求为背景，分别从线性系统与非线性系统、高斯噪声与非高斯噪声两个角度来进行滤波方法介绍。
　　1.1 Kalman滤波理论基础
　　信号是传递和运载信息的时间或空间函数。信号有两类，即确定性信号和随机信号。确定性信号的变化规律是既定的，可以表示为一确定的时间函数或空间函数，具有确定的频谱特性，如阶跃信号、脉宽固定的矩形脉冲信号、正余弦函数等，它们对于指定的某一时刻，可确定一相应的函数值。随机信号没有既定的变化规律，不能给出确定的时间或空间函数，在相同的初始条件和环境条件下，信号每次实现都不一样，如陀螺随机漂移、随机海浪等。随机信号没有确定的频谱特性，但是具有确定的功率谱，可以通过统计特性来描述其特征。
　　信号在检测与传输过程中不可避免地要受到外来干扰与设备内部噪声的影响，为获取所需信号，排除干扰，就要对信号进行滤波。信号的性质不同，获取有用信号的方法就不同，即滤波的手段不同。对于可用频谱特性描述的确定性信号，可根据信号所处频带的不同，设置具有相应频率特性的滤波器，如低通滤波器、高通滤波器、带通滤波器及带阻滤波器等，使有用信号尽可能无衰减地通过，而干扰信号受到抑制。这类滤波器可用物理的方法实现，即模拟滤波器，也可用计算机通过特定的算法实现，即数字滤波器。对确定性信号的滤波处理通常称为常规滤波。
　　随机信号具有确定的功率谱特性，可根据有用信号和干扰信号的功率谱设计滤波器。美国学者维纳(N. Wiener)等提出了Wiener滤波，他们通过功率谱分解设计滤波器，在对信号进行抑制和选通这一点上同常规滤波是相似的。由于在频域进行Wiener滤波器设计，要求解维纳-霍普方程，计算量较大。
　　Kalman滤波是匈牙利裔美国数学家鲁道夫？埃米尔？卡尔曼(Rudolf Emil Kalman，1930—2016)于1960 年提出的，是一种从与被提取信号有关的观测量中通过算法估计出所需信号的滤波方法。他把状态空间的概念引入随机估计理论中，把信号视为白噪声作用下的一个线性系统的输出，用状态方程来描述这种输入-输出关系，估计过程中利用系统状态方程、观测方程和白噪声激励(系统噪声和观测噪声)的统计特性构成滤波算法，由于所利用的信息都是时域内的变量，所以不但可以对平稳的、一维的随机过程进行估计，也可以对非平稳的、多维随机过程进行估计。这就避免了Wiener滤波在频域内设计时遇到的困难，适用范围比较广泛。
　　实际上，Kalman滤波是一套由计算机实现的实时递推算法，它所处理的对象是随机信号，利用系统噪声和观测噪声的统计特性，将系统的观测量作为滤波器的输入、所要估计值(系统的状态或参数)作为滤波器的输出，滤波器的输入与输出由时间更新和观测更新算法联系在一起，根据系统状态方程和观测方程估计出所有需要处理的信号。因此，此处所谈的Kalman滤波与常规滤波的含义与方法完全不同，实质上是一种*优估计方法。下面对Kalman滤波的基础理论——估计理论进行详细阐述。
　　1.1.1 滤波与估计
　　在随机控制和信息处理过程中，通常观测信息中不仅包含所需信号，而且还包含随机观测噪声和干扰信号。通过对一系列带有观测噪声和干扰信号的实际观测数据进行处理，从中得到所需要的各种参量的估计值，这就是估计问题。在工程实践中，经常遇到的估计问题有两类：①系统的结构参数部分或全部未知，有待确定；②实施*优控制需要随时了解系统的状态，而由于种种限制，系统中的一部分或全部状态变量不能直接测得。也就是这两类估计问题——参数估计和状态估计。
　　一般估计问题都是由估计验前信息、估计约束条件和估计准则三部分构成的。若设：
　　(1)为维未知状态或参数，为其估计值。
　　(2)为与有关的维观测向量，它与的关系可表示为
　　(1.1)
　　(3)为维观测噪声，它的统计规律部分或全部已知。
　　则一般地，估计问题可叙述为：给定观测向量和观测噪声向量的全部或部分统计规律，根据选定的准则和约束条件(1.1)，确定一个函数，使得它成为(在选定准则下)的*优估计，即
　　(1.2)
　　在应用中，总是希望估计出来的参数或状态越接近实际值越好，即得到状态或参数的*优估计。为了衡量估计的好坏，必须要有一个估计准则。估计准则以某种方式度量了估计的精确性，它体现了估计是否*优的含义。很显然，估计准则可能是各式各样的，*优估计不是唯一的，它随着准则不同而不同。因此，在估计时，要恰当地选择衡量估计效果的准则。估计准则一般用函数来表达，在估计问题中称这个函数为指标函数或损失函数。
　　一般来说，损失函数是根据验前信息选定的，而估计式是通过损失函数的极小化或极大化导出的。不同的损失函数，导致不同的估计方法。原则上，任何具有一定性质的函数都可用作损失函数。从估计理论的应用实践看，可行的损失函数只有少数几种。目前估计中常用的三类准则是直接误差准则、误差函数矩准则和直接概率准则。
　　直接误差准则：以某种形式的误差为自变量的函数作为损失函数的准则，如估计误差或对的拟合误差(是的函数)。在这类准则中，损失函数是误差的凸函数，估计式是通过损失函数的极小化导出的，而与观测噪声的统计特性无关。因此，这类准则特别适用于观测噪声统计规律未知的情况。*小二乘估计及其各种推广形式都是以误差的平方和*小作为估计准则。
　　误差函数矩准则：以直接误差函数矩作为损失函数的准则。特别地，可把损失函数选作直接误差函数，以其均值为零和方差*小为准则。在这类准则中，要求观测噪声的有关矩是已知的，显然它比直接误差准则要求更多的信息，因而可望具有更高的精度。*小方差估计、线性*小方差估计等都是属于这类准则的估计。
　　直接概率准则：这类准则的损失函数是以某种形式误差的概率密度函数构成的，有时也用熵函数构成。估计式由损失函数的极值条件导出。由于这类准则与概率密度有关，这就要求有关的概率密度函数存在，而且要知道它的形式。极大似然估计和极大验后估计就是这类准则的直接应用。
　　选取不同的估计准则，就有不同的估计方法，估计方法与估计准则是紧密相关的。相应于上述三类估计准则，常用的估计方法有*小二乘估计、线性*小方差估计、*小方差估计、极大似然估计及极大验后估计等。
　　在估计问题中，常考虑如下随机线性离散系统模型：
　　(1.3a)
　　(1.3b)
　　式中，下标k为采样时间；是系统的维状态序列；是系统的维观测序列；是系统的维随机干扰序列；是系统的维观测噪声序列；是系统的状态转移矩阵；是干扰输入矩阵；是观测矩阵。在以后的讨论中，省略条件。
　　根据状态向量和观测向量在时间上存在的不同对应关系，可以把估计问题分为滤波、预测和平滑，以式(1.3)所描述的随机线性离散系统为例，设表示根据时刻和以前时刻的观测值，对时刻状态做出的某种估计，则按照和的不同对应关系，分别叙述如下。
　　(1)当时，对的估计称为滤波，即依据过去直至现在的观测值来估计现在的状态。相应地，称为的*优滤波估计值，简记为。这类估计主要用于随机系统的实时控制。
　　(2)当时，对的估计称为预测或外推，即依据过去直至现在的观测值来预测未来的状态，并把称为的*优预测估计值。这类估计主要用于对系统未来状态的预测和实时控制。
　　(3)当时，对的估计称为平滑或内插，即依据过去直至现在的观测值来估计过去的历史状态，并称为的*优平滑估计值。这类估计广泛应用于通过分析实验或实验数据，对系统进行评估。
　　若把换成，换成，则上述分类对于连续时间系统同样适用。换句话说，线性系统的状态估计都可分成以上三类。
　　在预测、滤波和平滑三类状态估计问题中，预测是滤波的基础，滤波是平滑的基础。本书主要讨论滤波问题。
　　1.1.2 线性*小方差估计
　　线性*小方差估计，就是在已知被估计量和观测量的一阶矩、二阶矩，即均值，方差和协方差的情况下，假定所求的估计量是观测量的线性函数，以估计误差方差阵达到*小作为*优估计的性能指标(损失函数)的估计方法。
　　假定估计是观测量的线性函数，即设
　　(1.4)
　　式中，为与同维的非随机向量；为具有相应维数的非随机矩阵。
　　记估计误差为
　　则选择向量和矩阵，使得下列平均二次性能指标
　　(1.5)
　　达到极小，此时得到的*优估计就称为线性*小方差估计，并记为。
　　将使达到极小的和记为和，则有
　　(1.6)
　　因此，只要求解和，就可以由式(1.6)得到。
　　为了求和，将对和求偏导。由于是向量和矩阵的标量函数，考虑到微分运算和期望运算是可交换的，可得
　　(1.7)
　　(1.8)
　　令式(1.7)和式(1.8)等于零，即可求得和。令式(1.7)等于零，得
　　(1.9)
　　将式(1.9)代入式(1.8)，并令式(1.8)等于零，可得
　　即
　　所以
　　(1.10)
　　将式(1.10)代入式(1.9)得
　　(1.11)
　　将式(1.10)和式(1.11)代入式(1.6)得
　　(1.12)
　　式(1.12)就是由观测值 求 的线性*小方差估计的表达式。
　　线性*小方差估计 具有如下性质：
　　(1)线性*小方差估计 是无偏估计，即
　　(1.13)
　　(2)估计误差的方差阵为
　　(1.14)
　　(3)任何一种线性估计的误差方差阵都将大于或等于线性*小方差估计的误差方差阵。
　　该性质的说明如下：设 的任一线性估计可表示为 ，则此估计的误差方差阵为
　　如果令
　　则可得
　　(1.15)
　　显然，任一线性估计的误差方差阵与 和 的选择有关。由于式(1.15)等号右边的**项和第二项都是非负定的，因此
　　(1.16)
　　式(1.16)说明，任何一种线性估计的误差方差阵都将大于或等于线性*小方差估计的误差方差阵。由此可见，线性*小方差估计具有*小的方差阵。