第二章 贝叶斯状态估计与粒子滤波

第二章 贝叶斯状态估计与粒子滤波
视觉跟踪可视为状态估计问题[16,54]，即根据视觉目标在先前帧的状态信息估计其在当前帧的状态，从而实现视觉跟踪。

状态估计一直都是自动控制、通讯、航空与航天等领域的经典研究主题之一[69,70]。

贝叶斯状态估计是处理不确定性条件下状态估计问题的有力理论工具[21,22,71]。

为了有效处理非高斯、非线性状态估计问题，二十世纪九十年代人们提出了粒子滤波[19-22,71]，粒子滤波是基于Monte Carlo 随机模拟的贝叶斯滤波方法。

本章将简单介绍贝叶斯状态估计和粒子滤波相关理论问题。

首先，通过介绍贝叶斯状态估计相关理论，引出贝叶斯状态滤波问题及实现贝叶斯状态滤波的两大理论工具：卡尔曼系滤波器和粒子滤波。

然后，简单介绍了卡尔曼系滤波器的相关理论和算法。

最后，详细介绍了粒子滤波理论框架、收敛性问题及经典采样策略。

2.1 贝叶斯状态估计
估计理论是概率论和数理统计的一个分支，所研究的对象是随机现象。

它是根据受干扰的观测数据来估计关于随机变量、随机过程或系统的某些特性的一种数学方法[70]。

所谓估计，就是从带随机噪声干扰的观测信号中提取有用信息，可定义如下：
定义 2.1 如果假设被估计量为n 维向量()t X ，而其观测量为m 维向量()t Z ，且观测量与被估计量之间具有如下关系
()()(),t h t t =⎡⎤⎣⎦Z X V (2.1)
其中，[]h ⋅是已知的m 维向量函数，由观测方法决定；()t V 是观测误差向量，通常为一个随机过程。

那么，所谓估计问题，就是在时间区间[]0,t t 内对()t X 进行观测，从而在得到观测数据(){}0,t t ττ=≤≤Z Z 的情况下，要求构造一个观测数据的函数()ˆX Z 去估计()t X 的问题，并称()ˆX
Z 是()t X 的一个估计量，或称()t X 的估计为()ˆX Z [69,70]。

一般地，估计问题可以分为两类：状态估计和参数估计。

状态和参数的基本差别在于，前者是随时间变化的随机过程，后者是不随时间变化或随时间缓慢变化的随机变量。

因此，
可以说状态估计是动态估计，而参数估计是静态估计。

在此，主要讨论系统状态估计问题。

下面首先介绍最优估计和估计准则，然后在贝叶斯意义下描述状态估计问题。

2.1.1 最优估计与估计准则
一般地，在实际工程应用中总希望估计出来的参数或状态愈接近真实值愈好，即如何最优地利用系统观测数据得到一个最优估计量，这就是最优估计问题。

所谓最优状态估计，是指在某一确定的估计准则条件下，按照某种统计意义，使系统状态估计达到最优[70]。

因此，最优状态估计是针对某一估计准则而言的。

估计准则是衡量估计的好坏的，选择合理的估计准则是极其重要的。

可以说，估计准则在很大程度上决定了估计的性能、求解估计问题所使用的估计方法及估计量的性质等。

估计准则是多种多样的，但在贝叶斯意义下，统计估计准则可分为：贝叶斯估计准则与非贝叶斯估计准则[69,70]。

非贝叶斯估计准则常见的有最小二乘准则和极大似然准则；而贝叶斯估计准则常见的有极大后验准则和最小方差准则。

估计准则和估计方法是紧密相关的，选择不同的估计准则就对应不同的估计方法。

下面将简要介绍基于这几种估计准则的统计估计方法： 1. 最小二乘估计
最小二乘估计是法国数学家高斯（Gauss ）于1809年提出的，是一种使用最广泛的估计方法之一。

最小二乘估计可定义如下：
定义 2.2 设被估计量X 是非时变的n 维随机向量，如果对其进行k 次线性观测，则有
() 1,2,,i i i i k =+=Z H X V (2.2)
其中i Z 是m 维观测向量，i Η是m n ⨯观测矩阵，i V 是m 维的零均值观测误差向量。

如果将k 次线性观测简写成，
=+Z H X V (2.3)
其中12k ⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦Z Z Z Z ，12k ⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦H H H H ，12k ⎡⎤
⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦
V V V V 则Z 是一个km 维向量，H 是km n ⨯矩阵，V 是km 维的零均值观测误差向量。

当km n ≥时，
可根据Z 来估计X 。

如果要求选择X 的一个估计ˆX
，使得性能指标 ()()()
ˆˆˆJ T
=--X
Z HX Z HX (2.4)
或更一般形式的二次型性能指标
()()
()
ˆˆˆJ T
W =--X
Z HX W Z HX
(2.5) 达到极小，那么称这个估计ˆX
为X 的最小二乘估计或加权最小二乘估计，并记为或(
)ˆL S W X Z 。

其中W 为km km ⨯对称正定加权矩阵[69,70]。

对于最小二乘估计，作如下几点说明：
（1）Z 是所有观测数据的全体，因此最小二乘估计要求把所有观测数据都储存起来作统一处理，很难实现实时处理。

（2）最小二乘估计或加权最小二乘估计都是无偏估计。

（3）设观测误差的方差阵为{}Var =R V ，则可以证明，当选择加权矩阵1-=W R 时，能使加权最小二乘估计的方差阵达到最小[70]。

2. 极大似然估计
极大似然估计是以观测值出现的概率最大作为准则的，是应用非常广泛的参数估计方法。

1906年费希尔（Fisher ）首先使用这种估计方法，它是以似然函数概念为基础的。

极大似然估计可定义如下[69,70,72,73]：
定义 2.3 设X 为n 维被估计量，{};1,2,,i i k ==Z Z 为X 的k 次观测数据集，它是从同一个分布()p Z X 独立采样得到的（即独立同分布的）。

记
()()1
k
i
i L p ==
∏X Z
X (2.6)
称为样本{};1,2,,i i k ==Z Z
的似然函数。

如果样本集Z 的一个函数()1ˆˆ,,ML ML k =X X Z Z 满足：()
()ˆsup M L X L L =X X ，则成ˆML
X 为X 的极大似然估计[70]。

对于极大似然估计，可以证明，当观测次数k 趋于无限大时，极大似然估计量ˆML
X 是一种无偏估计量。

因此，极大似然估计是渐近无偏的。

此外，极大似然估计量可以是随机量，
也可以是非随机参数，适用范围较广。

3. 贝叶斯估计
贝叶斯估计是以贝叶斯统计为基础的，是当前最优估计的研究热点之一。

贝叶斯统计的主要优势在于能处理数据分析中的不确定性，包括被估计的参数以及模型中的任何不确定性。

在贝叶斯统计中，被估计量X 都被当作随机变量，它服从一定的分布，并认为已观测到的数据可以揭示这个分布的信息。

根据先验知识确定的被估计量X 的分布，称为先验分布()p X ；该分布将根据纳入的观测数据中的信息进行改进而得到后验分布()p X Z 。

从先验分布进化为后验分布是通过贝叶斯定理来实现的。

根据贝叶斯统计原理，贝叶斯估计可定义如下： 定义 2.4 设X 为被估计量，其先验分布为()p X ，1:k Z 为X 的k 个观测值，其条件概率密度函数为()1:k p Z X ，则可利用贝叶斯公式得到后验概率密度函数
()()()
()
1:1:1:k k k
p p p p =
Z X X X Z Z (2.7)
如果令()
ˆ,L X X 为损失函数，度量X 的一个估计ˆX 带来的损失，则后验分布()1:k p X Z 下的损失函数()
ˆ,L X X
的期望 ()
()()1:ˆˆ,,B k
E L L p d ⎡⎤=⎣⎦
⎰X X
X X
X Z X (2.8)
称为贝叶斯后验风险。

如果估计ˆB X 能使后验风险达到最小，则称ˆB
X 为X 的Bayes 估计[70]。

由式(2.8)可知，损失函数的选择非常重要，选择不同形式的损失函数，就可得到不同的贝叶斯估计结果。

下面将讨论两种情况：最大后验估计和最小方差估计。

（1）最大后验估计：如果令损失函数为01-风险，即
()
ˆ0 ˆ,ˆ1 L ⎧=⎪=⎨≠⎪⎩X X X X X X
(2.9)
于是，将式(2.9)代入式(2.8)则有
()
()()1:1:1:1:ˆ,arg m in arg m ax B k
k k
k E L p d p d ⎡⎤=≠=⎣⎦===⎰⎰X
X
X X
X x Z
z x X x Z
z x
(2.10)
显然，由式(2.10)可知，使贝叶斯后验风险达到最小，就相当于要求后验概率密度()1:k p X Z 达到最大[70]。

也就是说，“()
ˆ,B E L ⎡⎤⎣⎦
X X 达到最小”与“()1:k
p X Z 达到最大”是等价的。

因此，可以把“使后验概率密度函数()1:k p X Z 达到最大”作为估计准则来得到贝叶斯估计
ˆB
X ，并把这种估计称为最大后验估计，记为ˆM AP X 。

对于最大后验估计，将式(2.7)代入式(2.10)，即有
()
()()
1:1:ˆ,arg m ax ()
k B k
p E L p d p ⎡⎤=⎣⎦⎰X
z x X X x x z (2.11)
由式(2.11)可知，在对X 没有任何先验统计知识的情况下，最大后验估计就退化为极大似然估计，因此可以说，极大似然估计是一种特殊的最大后验估计，或者说是一种特殊的贝叶斯估计。

但是，在一般情况下，由于考虑了X 的先验知识（即先验分布()p X ），因此最大后验估计将优于极大似然估计。

（2）最小方差估计：如果令估计误差的二次型函数为损失函数，即
()
2ˆˆˆˆ,T
L ⎡⎤⎡⎤=-=--⎣⎦⎣⎦
W
X X
X X X X
W X X (2.12) 于是，将式(2.12)代入式(2.8)则可得到贝叶斯性能指标为
()
()1:ˆˆˆ,T
B k E L p d ⎡⎤⎡⎤⎡⎤=--⎣⎦⎣⎦
⎣⎦
⎰X X
X X W X X X Z X (2.13) 一般地，把“使式(2.13)所示的贝叶斯性能指标达到最小”作为估计准则所得到的贝叶斯估计
称为最小方差估计，记为ˆM V X 。

对于最小方差估计，作如下几点说明： ⅰ、最小方差估计为无偏估计。

ⅱ、可以证明，任何其他估计的均方误差阵或任何其他无偏估计的方差阵都将大于最小
方差估计的误差方差阵，即最优估计ˆM V X 具有最小的估计误差方差阵[70]。

2.1.2 贝叶斯意义下的状态估计
卡尔曼（Kalman ）开创性地将状态变量和状态空间概念引入到最优估计，提出了状态估计理论[74]。

从状态空间观点，状态是比信号更为广泛、更灵活的概念，非常适合处理多变
量系统，信号可以视为状态或状态的分量。

一般地，动态系统的状态可定义如下： 定义 2.5 把能完全确定动态系统运动特性的最小一组变量X ，称为该动态系统的状态；状态变量所构成的向量称为状态向量；状态向量所张成的空间称为状态空间。

对于实际系统而言，其状态很难直接获得或不允许直接测量，得到的只是与状态有关的一些观测数据，而且观测数据往往会受到随机噪声的干扰，是有观测误差的。

为了得到系统的状态，就只有根据这些观测数据构造或估计系统的状态，当然，系统状态的估计值应尽量接近实际状态，这就是所谓的系统状态估计问题[69,70]。

在统计理论里，贝叶斯统计是处理不确定性问题的有力工具[75]。

一直以来，贝叶斯状态估计是系统状态估计（特别是非线性、非高斯状态估计）的一大研究热点[21,48,71,76]。

下面将简要介绍离散系统的状态估计和贝叶斯意义下的状态估计： 1. 离散系统的状态估计
离散系统的状态估计可定义如下：
定义 2.6 设随机、离散系统S 的状态空间模型为
()()1k k k
k
k k F H -=+⎧⎪⎨
=+⎪⎩X X U Z X V (2.14) 其中，k X 为k 时刻（1k ≥）的系统状态向量，k U 为系统随机噪声，()F ⋅为系统状态转移模型；k Z 为k 时刻的系统观测向量，k V 为随机观测噪声，()H ⋅为系统观测模型。

如果对系统的状态向量进行k 次观测，从而得到观测序列{}1:12,,,k k =Z Z Z Z ，那么所谓离散系统得状态估计问题，就是要求根据整个观测数据1:k Z ，求得在j 时刻系统状态向量j X 的一个最
优估计量的问题，通常把所得到的估计量记为()ˆj k X
，并且按照j 和k 的不同关系，状态估计可分为三类：
1）j k > 称为预测（或外推）； 2）j k = 称为滤波；
3）j k < 称为平滑（或内插）。

[69, 70] 2. 贝叶斯意义下的状态估计与递推贝叶斯滤波
考虑式（2.14）所示状态空间模型建模的动态系统S 的状态估计问题。

如果将系统状态转移模型和观测模型概率化，则系统S 包含两个随机过程：状态过程和观测过程。

其中，状态过程{}
0,1,
k k =X 可视为具有初始分布()0p X 的离散马尔科夫链，且其马尔科夫转移核为
()1,1k k p k -≥X X ，称为状态转移概率；观测过程{}0,1,k k =Z 条件依赖于状态过程，其条件
分布为(),1k k p k ≥Z X ，称为似然函数（又称为似然比）。

于是，系统S 的状态过程和观测过程的统计特性可完全由初始分布()0p X 、状态转移概率()1k k p -X X 和似然函数
()k k p Z X 决定：
()()()
()
1010,, 1k k k k p p p k p --⎧=⎪≥⎨
⎪⎩X X X X X Z X (2.15) 对于式（2.14），如果k U 与k V 为独立同分布的噪声，且相互独立，统计特性已知（即()k p U ，()k p V 已知），则状态转移概率和似然函数可由噪声的分布给出：
()()()()
()()11k k k k k k k k k k p p F p p H --⎧=-⎪⎨
=-⎪⎩U V X X X X Z X Z X (2.16) 特别地，当噪声信号具有高斯分布，且均值和协方差阵为：()k k E =U q ，()k k E =V r ，()k k Var =U Q ，()k k Var =V R ，则式（2.16）可改写为：
()()()()
()()11;,;,k k k k k k k k k k k k p N F p N H --⎧=+⎪⎨
=+⎪⎩X X X X q Q Z X Z X r R (2.17) 其中，()N ⋅为高斯分布函数。

于是，根据贝叶斯估计准则（定义 2.4）和系统状态估计（定义 2.6），贝叶斯意义下的状态估计问题即为：给定一系列观测数据{}1:12,,,k k =Z Z Z Z ，求得一列“最优状态估计
序列”{}
0:01ˆˆˆˆ,,,k k k k k
=X X X X ，以使得式（2.8）定义的贝叶斯后验风险指标达到最小： (
)(
)
()()0:0:0:0:0:0:1:ˆˆarg m in ,ˆ arg m in
,k B k k
k
k
k
k E L L p d ⎡⎤=⎣⎦=⎰X X
X X X X
X X
Z X
(2.18)
对于许多实际问题，最感兴趣的是状态的当前值。

只考虑当前状态取值估计，这样的最优状态估计问题就变成一个最优滤波问题：在给定的观测数据{}1:12,,,k k =Z Z Z Z ，求得一个
最优的当前状态估计值ˆˆk k k
=X X ，使得满足贝叶斯后验风险指标： (
)(
)
()()1:ˆˆarg m in ,ˆ arg m in
,B k k k k
k
k
k
k E L L p d ⎡⎤=⎣⎦=⎰X X
X X X X
X X
Z X
(2.19)
由贝叶斯估计（见2.1.1节）可知，对于其两种估计方法（最大后验估计和最小方差估计），若分别知道()0:1:k k p X Z 和()1:k k p X Z 的显式解，则贝叶斯意义下的最优估计和最优滤波问题的递推显式解即可求得。

一般地，求解贝叶斯估计和滤波的后验分布的递推方程称为递推贝叶斯估计和滤波方程。

对于离散时间随机系统S ，在给定初始先验状态分布()0p X 条件下，()0:1:k k p X Z 的递推解可按如下序贯贝叶斯估计公式给出：
()()()
()
()10:1:0:11:11:1k k
k
k k k k k k k p p p p p ----=
Z X X
X X Z X Z Z Z (2.20)
其中，()1:1k k p -Z Z 为归一化常数。

在实际应用中，感兴趣的是当前状态的滤波估计，如何求得状态后验分布()1:k k p X Z 是贝叶斯滤波的核心问题。

对于贝叶斯滤波，可由如下两式递推求解：
（1）预测方程（Chapman-Kolmogorov 方程）：
()()()1:1111:11k k k
k k k k p p p d -----=
⎰X Z X
X X Z X (2.21)
（2）更新方程（贝叶斯推理）：
()()()
()
1:11:1:1k k
k
k k k k k p p p p --=
Z X X
Z X Z Z Z (2.22)
一般地，式（2.21）和式（2.22）被合称为贝叶斯滤波方程。

在理论意义上，式（2.20）与式（2.21）、式（2.22）分别构成了贝叶斯最优估计和贝叶斯最优滤波的递推最优求解方法。

但是，在一般情况下，它们的显式解是不能得到的。

因此，人们采用各种途径寻找贝叶斯滤波的近似数值求解方法。

如果系统具有线性和高斯性，则卡
尔曼滤波是求解最优贝叶斯滤波有效方法[69,70,74,77,78]。

为了处理非线性滤波问题，人们提出了扩展卡尔曼滤波[69,70,77-80]；同时，为了更好地处理非线性滤波问题，Julier等在卡尔曼滤波的理论框架下提出了Unscented卡尔曼滤波[52,53]。

但是，卡尔曼滤波、扩展卡尔曼滤波和Unscented卡尔曼滤波都不能有效解决非线性、非高斯情况下的贝叶斯滤波问题。

为了处理非线性和非高斯滤波问题，20世纪90年代人们提出了粒子滤波[19-22,71]。

粒子滤波给出了非线性和非高斯情况下最优贝叶斯滤波的近似数值解，当前已成为最优贝叶斯滤波理论领域的研究热点。

2.2 卡尔曼系滤波器
为了实现系统状态估计，在20世纪40年代，Wiener和Kolmogorov彼此独立地提出了一种最优线性滤波方法，称为维纳滤波[78]。

但维纳滤波的不足之处是：（1）求解比较复杂，很难得到解析解，而且求得的最优滤波器在工程上很难实现；（2）计算是非递推的，需存储全部观测数据，存储量大且实时性差；（3）不适用于非平稳随机过程的滤波。

在20世纪60年代，Kalman突破了维纳滤波的局限性，提出了在时域上的状态空间方法。

引入状态变量和状态空间概念，从而改变了对滤波问题的一般描述，即它不是要求直接给出信号过程的二阶特性或谱密度函数，而是把信号过程视为在白噪声作用下一个线性系统的输出。

在此基础上，利用Hilbert空间投影理论，Kalman提出了状态估计理论，称为卡尔曼滤波理论[74]。

卡尔曼滤波器给出了一套易于在计算机上实时实现的最优线性递推滤波算法，适合处理多变量系统和时变系统，适合处理非平稳随机过程，克服了维纳滤波理论的缺点和局限性[70,78]。

但是，卡尔曼滤波器只能有效处理线性高斯情况下的滤波问题。

在滤波器理论中，扩展卡尔曼滤波是应用最为广泛的一种非线性状态估计方法，其实质是将非线性的状态转移模型或观测模型线性化，然后利用卡尔曼预测方程和更新方程递推求解[79,80]。

然而，非线性系统状态空间模型的线性化经常会在状态估计中引入较大误差。

为了更好地处理非线性状态估计问题，Julier等在扩展卡尔曼滤波的理论框架下提出了Unscented卡尔曼滤波[52,53]。

在此，我们将卡尔曼滤波、扩展卡尔曼滤波和Unscented卡尔曼滤波统称为卡尔曼系滤波器。

本节将在最优贝叶斯滤波理论框架下简要介绍卡尔曼系滤波器理论。

2.2.1 经典卡尔曼滤波器
设动态系统S 具有线性、高斯性，则式（2.14）定义的状态空间模型可改写为：
1k k k k
k
k k k -=+⎧⎨
=+⎩X A X U Z C X V (2.23) 其中，k A 为系统状态转移矩阵，由线性状态转移模型()F ⋅确定；k C 为系统观测矩阵，由线性观测模型()H ⋅确定；系统噪声k U 和观测噪声k V 为零均值高斯白噪声（即k U 和k V 服从高斯分布），且其协方差阵为：()k k Var =U Q ，()k k Var =V R 。

令系统状态初始分布为高斯分布()0000;,N X X X P ，可以证明[]，若()11:1k k p --X Z 服从高斯分布
()()
11:111111;,k k k k k k k p N -------=X Z X X P (2.24)
则()1:1k k p -X Z 和()1:k k p X Z 也是高斯的，
()()
1:111;,k k k k k k k p N ---=X Z X X P (2.25) ()()
1:1;,k k k k k k k p N -=X Z X X P (2.26)
其中，();,N X X P 为高斯分布函数，X 为均值，P 为协方差。

于是，通过式（2.24）-（2.26）的卡尔曼滤波递推求解，即可实现式（2.21）和式（2.22）所定义的贝叶斯滤波递推求解[70]。

卡尔曼滤波可分为两部分：一步预测和观测更新。

其算法如下：
算法 2.1（卡尔曼滤波算法）：
（1）初始化：对于0k =，给定初始状态0X 的均值00X 和方差00P ； （2）递推求解：对于1,2,k = ，则有
（a ）一步预测：
111k k k k k ---=X A X (2.27) 1111T
k k k k k k k ----=+P A P A Q (2.28)
（b ）观测更新
()
1
11T T k k
k
k
k
k k k k ---=+K P C
C
P C R (2.29)
()
11k k k k k k k k k --=+-X X K Z C X (2.30)
11k k k k k k k k --=-P P K C P (2.31)
由于式（2.24）-（2.26）可由其均值和协方差阵完全表征，因此卡尔曼滤波是递推贝叶斯最优滤波的显式解。

也就是说，在给定线性高斯假设条件下，卡尔曼滤波与贝叶斯最优滤波是完全等价的。

2.2.2 扩展卡尔曼滤波器
对于式（2.14）定义的系统状态空间模型，若状态转移模型()F ⋅和状态观测模型()H ⋅是非线性的（但仍假设具有零均值高斯白噪声），则经典卡尔曼滤波不能实现最优滤波问题。

一般地，人们通过各种非线性近似求得近似解。

最基本的近似方法是泰勒近似法，其思路是：当状态的先验分布可用高斯分布近似时，状态的条件分布完全由其均值和协方差阵表征，若在状态的滤波值和预测值周围分别将状态转移方程和观测方程进行泰勒展开：
()()()
11111111k k k k F k k k k k k k k F --------=+-+∆-+X X A X X X X U (2.32) ()()()
111k k k k H k k k k k k k k H ---=+-+∆-+Z X C X X X X V (2.33)
其中，()111F k k k ---∆-X X 和()
111H k k k ---∆-X X 为被截掉的二阶以上的高阶项。

此种近似称为局部线性近似（也称为一阶泰勒近似），k A 和k C 是线性化的雅可比阵。

()()11
1
k k k k k k k k F H ---==∂⎧
=⎪⎪∂⎨
∂⎪
=⎪∂⎩
X X X A X X C X (2.34)
于是，非线性高斯系统S 的局部线性化系统为
1k k k k k
k k k k k
-⎧=++⎪⎨
=++⎪⎩X A X a U Z C X c V (2.35) 其中，在给定11k k --X 和1k k -X 时，()1111k k k k k k F ----=-a X A X 和()
11k k k k k k H --=-c X C X 分
别为确定性分量；()111k F k k k k ---=∆-+U X X U 和()
1k H k k k k -=∆-+V X X V 为随机噪声，且包含了线性化误差，因而已是非高斯的。

如果忽略线性化误差，即k k ≈U U 和k k
≈V V ，那么式（2.35）所示的局部线性化系统具有线性、高斯模型，从而可利用2.2.1节的线性高斯系统的卡尔曼滤波公式递推求解()1:k k p X Z ，即可近似假定状态的条件分布是高斯的：
()()
11:111111;,k k k k k k k p N -------X Z X X P (2.36) ()()
1:111;,k k k k k k k p N ---X Z X X P (2.37) ()()
1:1;,k k k k k k k p N -X Z X X P (2.38)
显然，式（2.36）-（2.38）中的参数可通过卡尔曼滤波公式递推近似求解。

此种非线性滤波方法称为扩展卡尔曼滤波（EKF ），其算法如下： 算法 2.2（EKF 算法）：
（1）初始化：对于0k =，给定状态先验高斯分布的均值00X 和方差00P ； （2）递推求解：对于1,2,k = ，则有
（a ）一步预测：
()11
k k k k F --=∂=
∂X X X A X
111k k k k k ---=X A X 1111T
k k k k k k k ----=+P A P A Q
（b ）观测更新
()1
k k k k H -=∂=
∂X X X C X
()
1
11T
T k k
k
k
k
k k k k ---=+K P C
C
P C R
()
11k k k k k k k k k --=+-X X K Z C X
11k k k k k k k k --=-P P K C P
从EKF 的机理分析可知，EKF 是一种局部次优的贝叶斯滤波估计，且当系统的非线性较强、状态的条件分布用高斯分布近似的误差较大时，采用EKF 近似非线性滤波可能导致较大的累积估计误差。

一般地，EKF 在应用中要注意两点[69,70,77,78]：
（1）基于泰勒展开的线性化方法易受参考点的影响。

EKF 是在当前估计值处进行泰勒展开的，并取其线性近似。

在EKF 递推计算过程中，卡尔曼滤波增益k K 依赖于当前的状态估计值。

如果当前估计值与真实值相差很大，则参考点的偏离将引起进一步的线性化误差以及不精确的卡尔曼滤波更新。

（2）由于EKF 使用了两个雅可比矩阵的计算，所以在EKF 应用时应注意状态转移模型和观测模型的连续性。

以上两点构成了EKF 的基本应用前提：小偏差初始条件和系统较弱的非线性；且()F ⋅和()H ⋅足够光滑，以确保雅可比阵k A 、k C 的存在性。

2.2.3 Unscented 卡尔曼滤波器
EKF 是在滤波器理论中应用较为广泛的一种非线性状态估计方法，但当非线性程度比较高时，非线性模型的线性化将导致较大的误差。

为了有效地处理非线性状态估计问题，Julier 等提出了Unscented 卡尔曼滤波（UKF ）[52,53,81]。

UKF 的核心是Unscented 变换，其使用一组适当选择的加权的离散采样（又称为Sigma 点）表征系统状态概率分布的均值和协方差，这组采样点根据非线性系统状态空间模型进行预测和观测，从而不需线性化[81,82]。

1. Unscented 变换与尺度Unscented 变换
Unscented 变换是一种近似计算经历了非线性变换的随机变量的统计特性的新方法，它建立的动机是：近似一个概率分布比近似一个任意的非线性变换（或函数）要容易[81-83]。

假设X 是均值为X 、协方差阵为XX P 的n 维随机向量，且X 经历非线性变换：
()g =Z X (2.39)
其中，()g ⋅为非线性函数。

为了估计随机向量Z 的均值Z 和协方差阵ZZ P ，Julier 和Uhlmann 提出了Unscented 变换（UT ）[52,82]。

其基本思想是：根据一种特定的、确定性的方法采样
21n +个加权随机样本{},i i i S W =X ，这组随机样本i S 的均值为X 、协方差阵为XX P ；然后
将非线性函数()g ⋅作用于每个样本得到变换后的一组随机样本i Z ，且i Z 能很好地表征随机变量Z 的统计特性。

UT 的具体步骤如下： （1）根据下列方程采样21n +个加权随机样本点，
0, 1,,, 1,,2i i
i i
i n
i n n
⎧
=⎪⎪
=+=⎨⎪
⎪=-
=+⎩
X X X X X X (2.40)
()()()0 12 1,,2
i
W n W n i n κκκ=+⎧⎪⎨
=+=⎪⎩ (2.41)
其中，i W 是样本点i X 权，且201n
i i W ==∑；κ是尺度因子，控制采样点i X 与样本均值X 之间
的距离；
i
是()n κ+XX P 的第i 列（或行）的矩阵平方根。

这些加权样本点称
为Sigma 点，并记为{},i i i S W =X 。

（2）将非线性函数()g ⋅作用于每个Sigma 点，则得到一组随机样本点i Z ：
()i i g =Z X
（3）估计随机向量Ζ的均值Z 和协方差阵ZZ P ，
()()20
20n i i i n
T
i i i i W W ==⎧
=⎪⎪⎨⎪=--⎪⎩
∑∑zz Z Z P Z Z Z Z (2.42) 显然地，UT 不需要将非线性函数线性化，也不需要计算雅可比矩阵。

而且可以证明[52]：UT 能精确估计任意非线性函数()g ⋅的二阶泰勒近似解；估计误差为三阶及三阶以上高阶矩项截断误差，且该估计误差被κ尺度化。

在UT 中，Sigma 点数随着状态空间的维数的增大而增大；且Sigma 点在状态空间的分布情况决定了UT 的性能。

特别地，在严重的非线性情况下Sigma 点的在状态空间的分布是
影响UT 性能的关键。

针对这个问题，Sigma 点被尺度化分布在状态空间里，第i 个
Sigma 点到均值X 的距离为i -X X ，且距离尺度化比为
0κ
<时，权00W <使
得估计的协方差阵可能是非半正定的。

鉴于此，Julier 提出了尺度化Unscented 变换（SUT ），
其不仅能保持估计的二阶精度，而且能使协方差估计是半正定的[82]。

在相同计算代价条件下，SUT 能部分地引入高阶矩信息，从而提高估计的精度。

对于SUT ，
Sigma 点按如下策略进行采样：
0, 1,,, 1,,2i i
i i
i n
i n n
⎧
=⎪⎪
=+=⎨⎪
⎪=-
=+⎩
X X X X X X (2.43)
()()()()()()
()
()()02
01 1,,212m c m c i i W n W n i n W W n λλλλαβλ⎧=+⎪
⎪=++-+=⎨⎪==+⎪⎩
(2.44) ()2
n n λα
κ=+- (2.45)
其中，()
m i
W 是均值估计权，()
c i
W 是协方差阵估计权；01α≤≤是尺度化因子，将控制Sigma
点的分布；0β≥是加权项，将引入高阶矩信息，提高估计精度（高斯先验下最优值为2β=）。

于是，随机向量Z 的均值Z 和协方差阵ZZ P 可按下式计算：
()()()()20
20n m i i i n
T c i i i
i W W ==⎧=⎪⎪⎨⎪=--⎪⎩
∑∑zz Z Z P Z Z Z Z (2.46) 2. Unscented 卡尔曼滤波器
Julier 等提出将SUT 和卡尔曼滤波结合实现高斯、非线性情况下的贝叶斯递推滤波问题，这种滤波称为Unscented 卡尔曼滤波器（UKF ）[53,81,83]。

如果假设式（2.14）定义的系统状态空间模型是高斯、非线性的，并将噪声变量引入到状态变量中产生扩展的状态变量
T
a T T T
k k k k ⎡⎤=⎣⎦
X X U V ，则UKF 算法具体如下：
算法 2.3（UKF 算法）： （1）初始化：对于0k =，令
0000a T
⎡⎤⎣⎦X =X ，0
000000
a ⎡⎤
⎢⎥
=⎢
⎥⎢⎥⎣⎦
P P Q R (2.47) 其中，0X 和0P 是初始状态的均值和协方差阵；系统噪声k U 和观测噪声k V 为零均值高斯白噪声，且其协方差阵为Q 和R 。

（2）递推估计：对于1,2,k = ，则有
（a ）计算Sigma 点
111111 a a a
k k k k k k ------⎡=±
⎣
X X X (2.48) （b ）UKF 预测
(
)
11111X
X
U
k k k k k k F -----=+X X X (2.49)
()
21,10a
n m X
i
k k i k k i W
--==
∑X X (2.50)
()
21,11,110
a
n T
c X X
i
k k i k k k k i k k k k i W
-----=⎡⎤⎡⎤=
--⎣⎦⎣⎦
∑P X X X X (2.51) (
)
1111X
V
k k k k k k H ----=+Z X X (2.52)
()
21,10
a
n m i
k k i k k i W
--==
∑Z Z (2.53)
（c ）UKF 更新
()
2,11,110a
k k n T
c i
i k k k k i k k k k i W
----=⎡⎤⎡⎤=
--⎣⎦⎣⎦
∑Z Z P Z Z Z Z (2.54) ()
2,11,110
a
k k n T
c X i
i k k k k i k k k k i W
----=⎡⎤⎡⎤=
--⎣⎦⎣⎦
∑X Z P X X Z Z (2.55) 1
k k k k k -=X Z Z Z K P P (2.56)
()
11k k k k k k k k --=+-X X K Z Z (2.57)
1k k T
k k k k k k -=-Z Z P P K P K (2.58)
其中，a n n n n =++X U V 是扩展状态空间a X 的维数，K 是卡尔曼增益矩阵。

与EKF 相比，UKF 实现高斯、非线性滤波不需计算雅可比阵，能实现任意高斯、非线性情况下的状态估计问题。

无论是在理论上还是在实际应用中，UKF 被证明都要优于EKF[52,53,81,83]。

2.3 粒子滤波
对于式（2.21）和式（2.22）定义的递推贝叶斯滤波问题，卡尔曼系滤波器在对状态后验分布()1:k k p X Z 递推求解中，仅对后验分布的一阶和二阶矩进行近似递推计算。

因此，卡尔曼系滤波器仅适用于高斯、线性系统或高斯、非线性系统的滤波问题。

对于非高斯、非线性系统，其状态分布实际上都有无穷个参数，于是仅在递推参数中传递两个低阶矩参数（或高斯分布假设）是不够的。

显然地，非参数估计方法是一种有效的途径，将可完全放弃对状态分布所作的高斯假设。

粒子滤波就是这样一种方法，采用序贯蒙特卡罗（Sequential Monte Carlo ）模拟来近似状态分布，并实现贝叶斯递推滤波。

粒子滤波器（又称为CONDENSATION 、Bootstrap Filter 或Sequential Monte Carlo Filter ）分别由信号处理[21,84]、计算机视觉[20,85]、统计[22,71]等领域独立地提出用以解决非高斯、非线性贝叶斯递推滤波问题。

粒子滤波是以Monte Carlo 随机模拟理论为基础，将系统状态后验分布用一组加权随机样本（称为粒子）近似表示，新的状态分布通过这些随机样本的贝叶斯递推估计。

粒子滤波主要包括三个步骤：（1）粒子采样，从建议分布（Proposal Distribution ）中抽取一组新的粒子；（2）粒子加权，根据观测概率分布和贝叶斯公式计算各个粒子的权值；（3）估计输出，输出系统状态的均值、协方差或高阶矩等。

此外，为了避免粒子滤波中出现的退化现象，重采样步骤经常被采用。

本节首先简单介绍蒙特卡罗随机模拟原理，在此基础上阐述标准粒子滤波的理论框架；并讨论标准粒子滤波的粒子退化和“样贫”问题；同时，简单讨论了粒子滤波的收敛性问题。

2.3.1 蒙特卡罗（Monte Carlo ）随机模拟。